private static final void areJointsTheSame(InverseDynamicsJoint originalJoint, InverseDynamicsJoint targetJoint) { if(!(originalJoint.getClass().equals(targetJoint.getClass()) && originalJoint.getName().equals(targetJoint.getName()))) { throw new RuntimeException(originalJoint.getName() + " differs from " + targetJoint); } } }
public static InverseDynamicsJoint[] findJointsWithNames(InverseDynamicsJoint[] allJoints, String... jointNames) { if (jointNames == null) return null; InverseDynamicsJoint[] ret = new InverseDynamicsJoint[jointNames.length]; int index = 0; for (InverseDynamicsJoint joint : allJoints) { for (String jointName : jointNames) { if (joint.getName().equals(jointName)) ret[index++] = joint; } } if (index != jointNames.length) throw new RuntimeException("Not all joints could be found"); return ret; }
public String toString() { String ret = getClass().getSimpleName() + ": "; for (int i = 0; i < joints.size(); i++) { ret += joints.get(i).getName(); if (i < joints.size() - 1) ret += ", "; else ret += "."; } return ret; } }
@Override public String toString() { String ret = getClass().getSimpleName() + ": "; for (int i = 0; i < joints.size(); i++) { ret += joints.get(i).getName(); if (i < joints.size() - 1) ret += ", "; else ret += "."; } return ret; } }
public String getParentJointName() { return contactingRigidBody.getParentJoint().getName(); }
/** * Creates a descriptive {@code String} for this Jacobian containing * information such as the {@code jacobianFrame}, the list of the joint names, * and the current value of the Jacobian matrix. * * @return a descriptive {@code String} for this Jacobian. */ @Override public String toString() { StringBuilder builder = new StringBuilder(); builder.append("Jacobian. jacobianFrame = " + jacobianFrame + ". Joints:\n"); for (InverseDynamicsJoint joint : unitTwistMap.keySet()) { builder.append(joint.getClass().getSimpleName() + " " + joint.getName() + "\n"); } builder.append("\n"); builder.append(jacobian.toString()); return builder.toString(); }
public void addJoint(InverseDynamicsJoint joint, DenseMatrix64F desiredAcceleration) { checkConsistency(joint, desiredAcceleration); joints.add(joint); jointNames.add(joint.getName()); desiredAccelerations.add().set(desiredAcceleration); }
private void addJoint(CollisionBoxProvider collissionBoxProvider, InverseDynamicsJoint joint) { List<CollisionShape> collisionMesh = collissionBoxProvider.getCollisionMesh(joint.getName()); if (collisionMesh != null) { trackingCollisionShapes.add(new TrackingCollisionShape(joint.getFrameAfterJoint(), collisionMesh)); } else { System.err.println(joint + " does not have a collission mesh"); } }
public void addJoint(InverseDynamicsJoint joint, DenseMatrix64F desiredVelocity) { checkConsistency(joint, desiredVelocity); joints.add(joint); jointNames.add(joint.getName()); desiredVelocities.add().set(desiredVelocity); }
public PushRobotController(FloatingRootJointRobot pushableRobot, FullRobotModel fullRobotModel) { this(pushableRobot, fullRobotModel.getChest().getParentJoint().getName(), new Vector3d(0, 0, 0.3)); }
public ForceSensorToJointTorqueProjector(String namePrefix, ForceSensorData forceSensorData, RigidBody sensorLinkBody) { registry = new YoVariableRegistry(namePrefix+getClass().getSimpleName()); this.forceSensorData = forceSensorData; //ground reaction wrench on joints yoTorqueInJoints = new ArrayList<>(); RigidBody currentBody = sensorLinkBody; for(int i=0;i<numberOfJointFromSensor;i++) { FrameVector jAxis = ((OneDoFJoint)currentBody.getParentJoint()).getJointAxis(); yoTorqueInJoints.add(new ImmutablePair<>(jAxis,new DoubleYoVariable("NegGRFWrenchIn"+ currentBody.getParentJoint().getName(), registry))); currentBody=currentBody.getParentJoint().getPredecessor(); } } @Override
public ForceSensorToJointTorqueProjector(String namePrefix, ForceSensorData forceSensorData, RigidBody sensorLinkBody) { registry = new YoVariableRegistry(namePrefix+getClass().getSimpleName()); this.forceSensorData = forceSensorData; //ground reaction wrench on joints yoTorqueInJoints = new ArrayList<>(); RigidBody currentBody = sensorLinkBody; for(int i=0;i<numberOfJointFromSensor;i++) { FrameVector jAxis = ((OneDoFJoint)currentBody.getParentJoint()).getJointAxis(); yoTorqueInJoints.add(new ImmutablePair<>(jAxis,new DoubleYoVariable("NegGRFWrenchIn"+ currentBody.getParentJoint().getName(), registry))); currentBody=currentBody.getParentJoint().getPredecessor(); } } @Override
public ForceSensorDefinition(String sensorName, RigidBody rigidBody, RigidBodyTransform transformFromSensorToParentJoint) { this.sensorName = sensorName; this.rigidBody = rigidBody; InverseDynamicsJoint parentJoint = rigidBody.getParentJoint(); this.parentJointName = parentJoint.getName(); this.transformFromSensorToParentJoint = new RigidBodyTransform(transformFromSensorToParentJoint); ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); sensorFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(sensorName + "Frame", frameAfterJoint, transformFromSensorToParentJoint); }
private void addJointHolders(List<RigidBody> rootBodies) { InverseDynamicsJoint[] joints = ScrewTools.computeSubtreeJoints(rootBodies); for (InverseDynamicsJoint joint : joints) { JointHolder jointHolder = JointHolderFactory.getJointHolder(joint); YoProtoHandshake.JointDefinition.Builder jointDefinition = YoProtoHandshake.JointDefinition.newBuilder(); jointDefinition.setName(joint.getName()); jointDefinition.setType(jointHolder.getJointType()); yoProtoHandshakeBuilder.addJoint(jointDefinition); jointHolders.add(jointHolder); } }
private void addJointHolders(List<RigidBody> rootBodies) { InverseDynamicsJoint[] joints = ScrewTools.computeSubtreeJoints(rootBodies); for (InverseDynamicsJoint joint : joints) { JointHolder jointHolder = JointHolderFactory.getJointHolder(joint); YoProtoHandshake.JointDefinition.Builder jointDefinition = YoProtoHandshake.JointDefinition.newBuilder(); jointDefinition.setName(joint.getName()); jointDefinition.setType(jointHolder.getJointType()); yoProtoHandshakeBuilder.addJoint(jointDefinition); jointHolders.add(jointHolder); } }
private void printLinkInformation(RigidBody link, StringBuffer buffer) { RigidBodyInertia inertia = link.getInertia(); InverseDynamicsJoint parentJoint = link.getParentJoint(); if (inertia != null) { double mass = inertia.getMass(); Vector3d comOffset = new Vector3d(); RigidBodyTransform comOffsetTransform = link.getBodyFixedFrame().getTransformToDesiredFrame(parentJoint.getFrameAfterJoint()); comOffsetTransform.getTranslation(comOffset); Matrix3d momentOfInertia = inertia.getMassMomentOfInertiaPartCopy(); buffer.append("Mass = " + mass + "\n"); buffer.append("comOffset = " + comOffset + "\n"); buffer.append("momentOfInertia = \n" + momentOfInertia + "\n"); } List<InverseDynamicsJoint> childrenJoints = link.getChildrenJoints(); for (InverseDynamicsJoint childJoint : childrenJoints) { String parentJointName; if (parentJoint != null) parentJointName = parentJoint.getName(); else parentJointName = "root joint"; buffer.append("Found Child Joint of " + parentJointName + ".\n"); printJointInformation(childJoint, buffer); } }
jointToAddExternalForcePoints = robot.getJoint(fullRobotModel.getPelvis().getParentJoint().getName());
private static CenterOfMassCalculator createCenterOfMassCalculatorInJointZUpFrame(RigidBody rootBody, boolean preserveY) { if (DEBUG) System.out.println("\nCenterOfMassCalibrationTool: rootBody = " + rootBody); InverseDynamicsJoint parentJoint = rootBody.getParentJoint(); if (DEBUG) System.out.println("parentJoint = " + parentJoint); ReferenceFrame jointFrame = parentJoint.getFrameAfterJoint(); if (DEBUG) System.out.println("jointFrame = " + jointFrame); String jointName = parentJoint.getName(); if (DEBUG) System.out.println("jointName = " + jointName); ReferenceFrame jointZUpFrame; if (preserveY) { jointZUpFrame = new ZUpPreserveYReferenceFrame(ReferenceFrame.getWorldFrame(), jointFrame, jointName + "ZUp"); } else { jointZUpFrame = new ZUpFrame(ReferenceFrame.getWorldFrame(), jointFrame, jointName + "ZUp"); } RigidBody[] rigidBodies = ScrewTools.computeSubtreeSuccessors(rootBody.getParentJoint()); CenterOfMassCalculator centerOfMassCalculator = new CenterOfMassCalculator(rigidBodies, jointZUpFrame); return centerOfMassCalculator; } }
@Override protected void setupGroundContactPointFootSwitches(QuadrantDependentList<FootSwitchInterface> footSwitches, double totalRobotWeight) { if (!simulatedRobot.hasValue()) { PrintTools.warn(this, "simulatedRobot is null, creating touchdown based foot switches."); setupTouchdownBasedFootSwitches(footSwitches, totalRobotWeight); return; } for (RobotQuadrant robotQuadrant : RobotQuadrant.values) { ContactablePlaneBody contactablePlaneBody = footContactableBodies.get().get(robotQuadrant); InverseDynamicsJoint parentJoint = contactablePlaneBody.getRigidBody().getParentJoint(); String jointName = parentJoint.getName(); String forceSensorName = contactablePlaneBody.getName() + "ForceSensor"; OneDegreeOfFreedomJoint forceTorqueSensorJoint = simulatedRobot.get().getOneDegreeOfFreedomJoint(jointName); List<GroundContactPoint> contactPoints = forceTorqueSensorJoint.getGroundContactPointGroup().getGroundContactPoints(); RigidBodyTransform transformToParentJoint = contactablePlaneBody.getSoleFrame().getTransformToDesiredFrame(parentJoint.getFrameAfterJoint()); WrenchCalculatorInterface wrenchCaluclator = new GroundContactPointBasedWrenchCalculator(forceSensorName, contactPoints, forceTorqueSensorJoint, transformToParentJoint); FootSwitchInterface footSwitch = new QuadrupedDebugFootSwitch(wrenchCaluclator, contactablePlaneBody, totalRobotWeight, registry); footSwitches.set(robotQuadrant, footSwitch); } }