/** *********************** 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); }