/** *********************** SCS ROBOT - Links ******************************* */
private void createAndAttachBodyLink(LinkNames linkName) {
Link link = new Link(LinkNames.BODY_LINK.getName());
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaBody);
link.setMass(RobotParameters.MASSES.get(LinkNames.BODY_LINK));
link.setComOffset(comOffsetBody);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCube(cubeX, cubeY, cubeZ, RobotParameters.APPEARANCE.get(LinkNames.BODY_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.7);
bodyJointSCS.setLink(link);
}
private void createAndAttachFootLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(LinkNames.FOOT_LINK.getName());
Matrix3d inertiaFoot = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaFoot);
link.setMass(RobotParameters.MASSES.get(LinkNames.FOOT_LINK));
link.setComOffset(comOffsetFoot);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.translate(footOffsetX, 0.0, 0.0);
linkGraphics.addCube(footX, footY, -footZ, RobotParameters.APPEARANCE.get(LinkNames.FOOT_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
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);
}
