private void createAndAttachFootRB(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
ScrewTools.addRigidBody(
linkName.getName(),
allLegJoints.get(robotSide).get(JointNames.KNEE),
inertiaBody,
footMass,
comOffsetFoot);
}
private void createAndAttachCylinderRB(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Matrix3d inertiaCylinder = createInertiaMatrixCylinder(linkName);
Vector3d comOffsetCylinder =
new Vector3d(0.0, 0.0, -RobotParameters.LENGTHS.get(linkName) / 2.0);
allLegRigidBodies
.get(robotSide)
.put(
linkName,
ScrewTools.addRigidBody(
linkName.getName(),
allLegJoints.get(robotSide).get(jointName),
inertiaCylinder,
bodyMass,
comOffsetCylinder));
}
private void createAndAttachCylinderLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(linkName.getName());
Matrix3d inertiaCylinder = createInertiaMatrixCylinder(linkName);
link.setMomentOfInertia(inertiaCylinder);
link.setMass(RobotParameters.MASSES.get(linkName));
link.setComOffset(new Vector3d(0.0, 0.0, -RobotParameters.LENGTHS.get(linkName) / 2.0));
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCylinder(
-RobotParameters.LENGTHS.get(linkName),
RobotParameters.RADII.get(linkName),
RobotParameters.APPEARANCE.get(linkName));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
/** *********************** ID ROBOT - Rigid bodies ******************************* */
private void createAndAttachBodyRB(LinkNames linkName, SixDoFJoint bodyJointID) {
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
ScrewTools.addRigidBody(linkName.getName(), bodyJointID, inertiaBody, bodyMass, comOffsetBody);
}