public FullQuadrupedRobotModelFromDescription(RobotQuadrant[] robotQuadrants, RobotDescription description, QuadrupedJointNameMap sdfJointNameMap,
String[] sensorLinksToTrack)
{
super(description, sdfJointNameMap, sensorLinksToTrack);
this.robotQuadrants = robotQuadrants;
for (OneDoFJointBasics oneDoFJoint : getOneDoFJoints())
{
QuadrupedJointName quadrupedJointName = sdfJointNameMap.getJointNameForSDFName(oneDoFJoint.getName());
jointNameOneDoFJointBiMap.put(quadrupedJointName, oneDoFJoint);
JointLimit jointLimit = new JointLimit(oneDoFJoint);
jointLimits.put(quadrupedJointName, jointLimit);
jointLimitData.put(oneDoFJoint, new JointLimitData(oneDoFJoint));
}
for (RobotQuadrant robotQuadrant : robotQuadrants)
{
RigidBodyTransform soleToParentTransform = sdfJointNameMap.getSoleToParentFrameTransform(robotQuadrant);
MovingReferenceFrame soleFrame = MovingReferenceFrame
.constructFrameFixedInParent(robotQuadrant.toString() + "SoleFrame", getEndEffectorFrame(robotQuadrant, LimbName.LEG), soleToParentTransform);
soleFrames.put(robotQuadrant, soleFrame);
}
}