public MovingReferenceFrame getFrameAfterParentJoint() { return ankle.getFrameAfterJoint(); }
public AfterJointReferenceFrameNameMap(RigidBodyBasics base) { for(JointBasics joint : base.childrenSubtreeIterable()) { afterJointReferenceFrames.put(joint.getFrameAfterJoint().getName(), joint.getFrameAfterJoint()); } }
@Override public ReferenceFrame getHeadBaseFrame() { if(head != null) { JointBasics headJoint = head.getParentJoint(); return headJoint.getFrameAfterJoint(); } return null; }
private ReferenceFrame createOffsetFrame(JointBasics previousJoint, RigidBodyTransform transformToParent, String frameName) { ReferenceFrame parentFrame = previousJoint.getFrameAfterJoint(); ReferenceFrame beforeJointFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent(frameName, parentFrame, transformToParent); return beforeJointFrame; } }
@Override public ReferenceFrame getHeadBaseFrame() { return head.getParentJoint().getFrameAfterJoint(); }
protected void checkLinkIsNeededForSensor(JointBasics joint, JointDescription jointDescription) { if(sensorLinksToTrack != null) { for(int i = 0; i < sensorLinksToTrack.length; i++) { if(sensorLinksToTrack[i].equalsIgnoreCase(jointDescription.getName())); { sensorFrames.put(jointDescription.getName(),joint.getFrameAfterJoint()); } } } }
/** {@inheritDoc} */ @Override public MovingReferenceFrame getEndEffectorFrame(RobotSide robotSide, LimbName limbName) { return getEndEffector(robotSide, limbName).getParentJoint().getFrameAfterJoint(); }
private void addJoint(CollisionBoxProvider collissionBoxProvider, JointBasics 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"); } }
@Override public RigidBodyTransform getJointTransform3D() { return new RigidBodyTransform(jointToWrap.getFrameAfterJoint().getTransformToParent()); } };
public WholeBodyTrajectoryToolboxCommandConverter(RigidBodyBasics rootBody) { List<ReferenceFrame> referenceFrames = new ArrayList<>(); for (JointBasics joint : rootBody.childrenSubtreeIterable()) { referenceFrames.add(joint.getFrameAfterJoint()); referenceFrames.add(joint.getFrameBeforeJoint()); } for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) { referenceFrames.add(rigidBody.getBodyFixedFrame()); } referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(referenceFrames); for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody); }
public KinematicsToolboxCommandConverter(RigidBodyBasics rootBody) { List<ReferenceFrame> referenceFrames = new ArrayList<>(); for (JointBasics joint : rootBody.childrenSubtreeIterable()) { referenceFrames.add(joint.getFrameAfterJoint()); referenceFrames.add(joint.getFrameBeforeJoint()); } for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) { referenceFrames.add(rigidBody.getBodyFixedFrame()); } referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(referenceFrames); for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody); }
public KinematicsPlanningToolboxCommandConverter(RigidBodyBasics rootBody) { List<ReferenceFrame> referenceFrames = new ArrayList<>(); for (JointBasics joint : rootBody.childrenSubtreeIterable()) { referenceFrames.add(joint.getFrameAfterJoint()); referenceFrames.add(joint.getFrameBeforeJoint()); } for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) { referenceFrames.add(rigidBody.getBodyFixedFrame()); } referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(referenceFrames); for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody); }
@Override public MovingReferenceFrame getEndEffectorFrame(RobotQuadrant robotQuadrant, LimbName limbName) { if (hasQuadrant(robotQuadrant)) return getEndEffector(robotQuadrant, limbName).getParentJoint().getFrameAfterJoint(); else return null; }
public IMUDefinition(String name, RigidBodyBasics rigidBody, RigidBodyTransform transformFromIMUToJoint) { this.name = name; this.rigidBody = rigidBody; this.transformFromIMUToJoint = new RigidBodyTransform(transformFromIMUToJoint); ReferenceFrame frameAfterJoint = rigidBody.getParentJoint().getFrameAfterJoint(); imuFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent(name, frameAfterJoint, transformFromIMUToJoint); }
public ForceSensorDefinition(String sensorName, RigidBodyBasics rigidBody, RigidBodyTransform transformFromSensorToParentJoint) { this.sensorName = sensorName; this.rigidBody = rigidBody; JointBasics parentJoint = rigidBody.getParentJoint(); this.parentJointName = parentJoint.getName(); this.transformFromSensorToParentJoint = new RigidBodyTransform(transformFromSensorToParentJoint); ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); sensorFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent(sensorName + "Frame", frameAfterJoint, transformFromSensorToParentJoint); }
private void storeJointState() { MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.ACCELERATION, storedJointDesiredAccelerations); MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.VELOCITY, storedJointVelocities); for (JointBasics joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getJointTau(0, tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForce.SIZE, 1); DenseMatrix64F motionSubspace = new DenseMatrix64F(6, joint.getDegreesOfFreedom()); joint.getMotionSubspace(motionSubspace); CommonOps.mult(motionSubspace, tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.setIncludingFrame(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getFrameAfterJoint()); } }
public void holdCurrentPelvisHeight() { yoDesiredPelvisPosition.setFromReferenceFrame(fullRobotModel.getPelvis().getParentJoint().getFrameAfterJoint()); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
private RigidBody(String bodyName, JointBasics parentJoint, RigidBodyTransform inertiaPose) { if (bodyName == null) throw new IllegalArgumentException("Name can not be null"); name = bodyName; this.parentJoint = parentJoint; ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); bodyFixedFrame = MovingReferenceFrame.constructFrameFixedInParent(bodyName + "CoM", frameAfterJoint, inertiaPose); inertia = new SpatialInertia(bodyFixedFrame, bodyFixedFrame); // inertia should be expressed in body frame, otherwise it will change inertia.getBodyFrame().checkReferenceFrameMatch(inertia.getReferenceFrame()); parentJoint.setSuccessor(this); nameId = parentJoint.getPredecessor().getNameId() + NAME_ID_SEPARATOR + bodyName; }
public ScriptedFootstepGenerator(HumanoidReferenceFrames referenceFrames, FullHumanoidRobotModel fullRobotModel, WalkingControllerParameters walkingControllerParameters) { this.bipedFeet = setupBipedFeet(referenceFrames, fullRobotModel, walkingControllerParameters); for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics foot = bipedFeet.get(robotSide).getRigidBody(); ReferenceFrame ankleFrame = foot.getParentJoint().getFrameAfterJoint(); RigidBodyTransform ankleToSole = new RigidBodyTransform(); ReferenceFrame soleFrame = referenceFrames.getSoleFrame(robotSide); ankleFrame.getTransformToDesiredFrame(ankleToSole, soleFrame); transformsFromAnkleToSole.put(robotSide, ankleToSole); } }
private static CenterOfMassCalculator createCenterOfMassCalculatorInJointZUpFrame(RigidBodyBasics rootBody, boolean preserveY) { if (DEBUG) System.out.println("\nCenterOfMassCalibrationTool: rootBody = " + rootBody); JointBasics 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"); } CenterOfMassCalculator centerOfMassCalculator = new CenterOfMassCalculator(rootBody, jointZUpFrame); return centerOfMassCalculator; } }