How to use

getFrameAfterJoint

method

in

us.ihmc.robotics.screwTheory.InverseDynamicsJoint

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;
}