Vector3D jointAxis = new Vector3D(joint.getAxisInModelFrame()); Vector3D offset = new Vector3D(joint.getOffsetFromParentJoint()); visualTransform.setRotation(joint.getLinkRotation()); String sanitizedJointName = ModelFileLoaderConversionsHelper.sanitizeJointName(joint.getName()); switch (joint.getType()) if (joint.hasLimits() && jointNameMap != null) pinJoint.setLimitStops(joint.getLowerLimit(), joint.getUpperLimit(), 10.0, 2.5); if ((joint.getContactKd() == 0.0) && (joint.getContactKp() == 0.0)) pinJoint.setLimitStops(joint.getLowerLimit(), joint.getUpperLimit(), kLimit, bLimit); pinJoint.setLimitStops(joint.getLowerLimit(), joint.getUpperLimit(), 0.0001 * joint.getContactKp(), 0.1 * joint.getContactKd()); if (!Double.isNaN(joint.getVelocityLimit()) && joint.getVelocityLimit() >= 0.0) pinJoint.setVelocityLimits(joint.getVelocityLimit(), 0.0); pinJoint.setDamping(joint.getDamping()); pinJoint.setStiction(joint.getFriction()); if (!Double.isNaN(joint.getEffortLimit()) && joint.getEffortLimit() >= 0.0) pinJoint.setEffortLimit(joint.getEffortLimit());
public void addContactSensor(String sensorName, String parentJointName, ContactSensorType type) { SDFContactSensor sdfContactSensor = new SDFContactSensor(sensorName, parentJointName, type); joints.get(parentJointName).addContactSensor(sdfContactSensor); }
public void addForceSensor(String sensorName, String parentJointName, RigidBodyTransform transformToParentJoint) { SDFForceSensor sdfForceSensor = new SDFForceSensor(sensorName, transformToParentJoint); if(joints.get(parentJointName) != null) joints.get(parentJointName).addForceSensor(sdfForceSensor); }
@Override public Graphics3DObject getGraphicsObject(String name) { for(SDFLinkHolder linkHolder : rootLinks) { if(linkHolder.getName().equals(name)) { return new SDFGraphics3DObject(linkHolder.getVisuals(), resourceDirectories); } } SDFJointHolder joint = joints.get(name); RigidBodyTransform visualTransform = new RigidBodyTransform(); visualTransform.setRotation(joint.getLinkRotation()); return new SDFGraphics3DObject(joint.getChildLinkHolder().getVisuals(), resourceDirectories, visualTransform); }
private void checkJointChildren(SDFJointHolder joint) SDFLinkHolder link = joint.getChildLinkHolder(); for (Collision collision : link.getCollisions()) addSimulationContactPoint(joint.getName(), gcOffset); if (joint.getName().equals(jointMap.getJointBeforeFootName(robotSide))) break; else if (joint.getName().equals(jointMap.getJointBeforeHandName(robotSide))) break; else if (joint.getName().equals(jointMap.getChestName())) break; else if (joint.getName().equals(jointMap.getPelvisName())) break; else if (joint.getName().equals(jointMap.getNameOfJointBeforeThighs().get(robotSide))) System.err.println("Contacts with '" + joint.getName() + "' are not supported (" + name + ")");
RigidBodyTransform modelToParentLink = getParentLinkHolder().getTransformFromModelReferenceFrame(); RigidBodyTransform modelToChildLink = getChildLinkHolder().getTransformFromModelReferenceFrame(); if (parentJoint != null) rotationTransform.setRotation(parentJoint.getLinkRotation()); parentLinkToParentJoint = parentJoint.getTransformFromChildLink();
private void addForceSensorsIncludingDescendants(SDFJointHolder joint, JointNameMap jointNameMap) { addForceSensor(joint, jointNameMap); for (SDFJointHolder child : joint.getChildLinkHolder().getChildren()) { addForceSensorsIncludingDescendants(child, jointNameMap); } }
private void addForceSensor(SDFJointHolder joint, JointNameMap jointNameMap) { if (joint.getForceSensors().size() > 0) { String[] jointNamesBeforeFeet = jointNameMap.getJointNamesBeforeFeet(); String jointName = joint.getName(); String sanitizedJointName = ModelFileLoaderConversionsHelper.sanitizeJointName(jointName); JointDescription scsJoint = jointDescriptions.get(sanitizedJointName); boolean jointIsParentOfFoot = false; for (int i = 0; i < jointNamesBeforeFeet.length; i++) { if (jointName.equals(jointNamesBeforeFeet[i])) { jointIsParentOfFoot = true; } } for (SDFForceSensor forceSensor : joint.getForceSensors()) { ForceSensorDescription forceSensorDescription = new ForceSensorDescription(forceSensor.getName(), forceSensor.getTransform()); forceSensorDescription.setUseGroundContactPoints(jointIsParentOfFoot); forceSensorDescription.setUseShapeCollision(useShapeCollision); scsJoint.addForceSensor(forceSensorDescription); } } } }
private boolean isJointInNeedOfReducedGains(SDFJointHolder pinJoint) { String jointName = pinJoint.getName(); return jointName.contains("f0") || jointName.contains("f1") || jointName.contains("f2") || jointName.contains("f3") || jointName.contains("palm") || jointName.contains("finger"); }
try SDFJointHolder jointHolder = new SDFJointHolder(sdfJoint, links.get(parent), links.get(child)); if(this.descriptionMutator != null) for (SDFContactSensor sdfContactSensor : jointHolder.getContactSensors()) for (SDFForceSensor sdfForceSensor : jointHolder.getForceSensors()) joint.getValue().calculateTransformToParentJoint();
public SDFJointHolder(SDFJoint sdfJoint, SDFLinkHolder parent, SDFLinkHolder child) throws IOException name = createValidVariableName(sdfJoint.getName()); String typeString = sdfJoint.getType(); double sdfLowerLimit = Double.parseDouble(sdfJoint.getAxis().getLimit().getLower()); setLimits(sdfLowerLimit, sdfUpperLimit); child.setJoint(this); calculateContactGains();
private void checkJointChildren(SDFJointHolder joint) SDFLinkHolder link = joint.getChildLinkHolder(); for (Collision collision : link.getCollisions()) addSimulationContactPoint(joint.getName(), gcOffset); if (joint.getName().equals(jointMap.getJointBeforeFootName(robotSide))) break; else if (joint.getName().equals(jointMap.getJointBeforeHandName(robotSide))) break; else if (joint.getName().equals(jointMap.getChestName())) break; else if (joint.getName().equals(jointMap.getPelvisName())) break; else if (joint.getName().equals(jointMap.getNameOfJointBeforeThighs().get(robotSide))) System.err.println("Contacts with '" + joint.getName() + "' are not supported (" + name + ")");
@Override public ArrayList<CollisionMeshDescription> getCollisionObjects(String name) { // TODO: SDF collision stuff to RobotDescription collision stuff. for(SDFLinkHolder linkHolder : rootLinks) { if(linkHolder.getName().equals(name)) { SDFGraphics3DObject sdfGraphics3DObject = new SDFGraphics3DObject(linkHolder.getCollisions(), resourceDirectories); ArrayList<CollisionMeshDescription> collisionMeshDescriptions = new ArrayList<CollisionMeshDescription>(); //TODO: Figure out and add the collision meshes... return collisionMeshDescriptions; } } SDFGraphics3DObject sdfGraphics3DObject = new SDFGraphics3DObject(joints.get(name).getChildLinkHolder().getCollisions(), resourceDirectories); CollisionMeshDescription collisionMeshDescription = new CollisionMeshDescription(); ArrayList<CollisionMeshDescription> collisionMeshDescriptions = new ArrayList<CollisionMeshDescription>(); //TODO: Figure out and add the collision meshes... return collisionMeshDescriptions; }
public void setLimits(double lowerLimit, double upperLimit) { if (upperLimit > lowerLimit) { hasLimits = true; this.upperLimit = upperLimit; this.lowerLimit = lowerLimit; } else { hasLimits = false; this.upperLimit = Double.POSITIVE_INFINITY; this.lowerLimit = Double.NEGATIVE_INFINITY; PrintTools.debug(DEBUG, this, getName() + " has invalid joint limits. LowerLimit = " + lowerLimit + ", UpperLimit = " + upperLimit + ". Using LowerLimit = " + lowerLimit + ", UpperLimit = " + upperLimit + " instead."); } } }
recursivelyAddLinks(joint.getName(), joint.getChildLinkHolder());
recursivelyAddLinks(joint.getChildLinkHolder(), modelTransform, useCollisionMeshes);
recursivelyAddLinks(joint.getName(), joint.getChildLinkHolder());