public ReferenceFrame getFrameAfterParentJoint() { return ankle.getFrameAfterJoint(); }
@Override public ReferenceFrame getFrameAfterParentJoint() { return rigidBody.getParentJoint().getFrameAfterJoint(); }
public AfterJointReferenceFrameNameMap(RigidBody base) { InverseDynamicsJoint[] joints = ScrewTools.computeSubtreeJoints(base); for(InverseDynamicsJoint joint : joints) { afterJointReferenceFrames.put(joint.getFrameAfterJoint().getName(), joint.getFrameAfterJoint()); } }
@Override public ReferenceFrame getFrameAfterParentJoint() { return rigidBody.getParentJoint().getFrameAfterJoint(); }
public YoPointPositionDataObject(String namePrefix, RigidBody body, YoVariableRegistry registry) { this(namePrefix, body.getParentJoint().getFrameAfterJoint(), registry); }
public ReferenceFrame getEstimationFrame() { return estimationLink.getParentJoint().getFrameAfterJoint(); }
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 YoPointVelocityDataObject(String namePrefix, RigidBody body, YoVariableRegistry registry) { this(namePrefix, body.getParentJoint().getFrameAfterJoint(), registry); rigidBodyName = body.getName(); }
public IMUDefinition(String name, RigidBody rigidBody, RigidBodyTransform transformFromIMUToJoint) { this.name = name; this.rigidBody = rigidBody; this.transformFromIMUToJoint = new RigidBodyTransform(transformFromIMUToJoint); ReferenceFrame frameAfterJoint = rigidBody.getParentJoint().getFrameAfterJoint(); imuFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(name, frameAfterJoint, transformFromIMUToJoint); }
public static RigidBody addRigidBody(String name, InverseDynamicsJoint parentJoint, Matrix3d momentOfInertia, double mass, Vector3d centerOfMassOffset) { String comFrameName = name + "CoM"; ReferenceFrame comFrame = createOffsetFrame(parentJoint.getFrameAfterJoint(), centerOfMassOffset, comFrameName); RigidBodyInertia inertia = new RigidBodyInertia(comFrame, momentOfInertia, mass); RigidBody ret = new RigidBody(name, inertia, parentJoint); return ret; }
public static RigidBody addRigidBody(String name, InverseDynamicsJoint parentJoint, Matrix3d momentOfInertia, double mass, RigidBodyTransform inertiaPose) { String comFrameName = name + "CoM"; ReferenceFrame comFrame = createOffsetFrame(parentJoint.getFrameAfterJoint(), inertiaPose, comFrameName); RigidBodyInertia inertia = new RigidBodyInertia(comFrame, momentOfInertia, mass); RigidBody ret = new RigidBody(name, inertia, parentJoint); return ret; }
private final static ListOfPointsContactablePlaneBody createListOfPointsContactablePlaneBody(String name, RigidBody body, RigidBodyTransform contactPointsTransform, List<Point2d> contactPoints) { ReferenceFrame parentFrame = body.getParentJoint().getFrameAfterJoint(); ReferenceFrame contactPointsFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(name, parentFrame, contactPointsTransform); ListOfPointsContactablePlaneBody ret = new ListOfPointsContactablePlaneBody(body, contactPointsFrame, contactPoints); return ret; } }
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); }
public static Footstep generateStandingFootstep(RobotSide side, RigidBody foot, ReferenceFrame soleFrame) { FramePose footFramePose = new FramePose(foot.getParentJoint().getFrameAfterJoint()); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose); Footstep footstep = new Footstep(foot, side, soleFrame, footstepPoseFrame, trustHeight); return footstep; }
public SCSPointCloudLidarReceiver(String lidarName, ObjectCommunicator scsSensorsCommunicator, PointCloudDataReceiver pointCloudDataReceiver) { this.pointCloudDataReceiver = pointCloudDataReceiver; this.lidarFrame = pointCloudDataReceiver.getLidarFrame(lidarName); RigidBodyTransform lidarBaseToSensorTransform = pointCloudDataReceiver.getLidarToSensorTransform(lidarName); ReferenceFrame lidarAfterJointFrame = pointCloudDataReceiver.getLidarJoint(lidarName).getFrameAfterJoint(); this.lidarScanFrame = ReferenceFrame .constructBodyFrameWithUnchangingTransformToParent("lidarScanFrame", lidarAfterJointFrame, lidarBaseToSensorTransform); scsSensorsCommunicator.attachListener(SimulatedLidarScanPacket.class, this); }
public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis, boolean isPartOfClosedKinematicLoop) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; return new PassiveRevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis), isPartOfClosedKinematicLoop); }
public static RevoluteJoint addRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; RevoluteJoint joint = new RevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis)); return joint; }
public static PrismaticJoint addPrismaticJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; PrismaticJoint joint = new PrismaticJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis)); return joint; }
private Footstep createFootstepAtCurrentLocation(RobotSide robotSide) { ContactablePlaneBody foot = feet.get(robotSide); ReferenceFrame footReferenceFrame = foot.getRigidBody().getParentJoint().getFrameAfterJoint(); FramePose framePose = new FramePose(footReferenceFrame); framePose.changeFrame(worldFrame); PoseReferenceFrame poseReferenceFrame = new PoseReferenceFrame("poseReferenceFrame", framePose); boolean trustHeight = true; Footstep footstep = new Footstep(foot.getRigidBody(), robotSide, foot.getSoleFrame(), poseReferenceFrame, trustHeight); return footstep; }
private static RigidBody cloneRigidBody(RigidBody original, String cloneSuffix, InverseDynamicsJoint parentJointOfClone) { FramePoint comOffset = new FramePoint(); original.getCoMOffset(comOffset); comOffset.changeFrame(original.getParentJoint().getFrameAfterJoint()); String nameOriginal = original.getName(); Matrix3d massMomentOfInertiaPartCopy = original.getInertia().getMassMomentOfInertiaPartCopy(); double mass = original.getInertia().getMass(); Vector3d comOffsetCopy = comOffset.getVectorCopy(); RigidBody clone = ScrewTools.addRigidBody(nameOriginal + cloneSuffix, parentJointOfClone, massMomentOfInertiaPartCopy, mass, comOffsetCopy); return clone; }