@Before public void setUp() { elevator = new RigidBody("elevator", worldFrame); }
public TestingRobotModel() { worldFrame = ReferenceFrame.getWorldFrame(); elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); rootJoint = new SixDoFJoint("rootJoint", elevator); body = new RigidBody("body", rootJoint, Ixx, Iyy, Izz, mass, comOffset); bodyFrame = rootJoint.getFrameAfterJoint(); }
private static RigidBodyBasics createAndAttachCylinderRB(String name, RevoluteJoint parentJoint) { Matrix3D inertiaCylinder = RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidCylinder(1.0, 1.0, 1.0, Axis.Z); return new RigidBody(name, parentJoint, inertiaCylinder, 1.0, new Vector3D()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddRigidBody_String_InverseDynamicsJoint_Matrix3d_double_Vector3d() { String name = "body"; RigidBodyBasics predecessor = new RigidBody("Predecessor", theFrame); PlanarJoint parentJoint = new PlanarJoint(name, predecessor); Matrix3D momentOfInertia = new Matrix3D(); double mass = random.nextDouble(); RigidBodyBasics body = new RigidBody(name, parentJoint, momentOfInertia, mass, X); assertEquals("Should be equal", name, body.getName()); assertTrue(parentJoint.equals(body.getParentJoint())); }
private RigidBodyBasics createUpperArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("shoulderPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, THIGH_LENGTH / 2.0); return new RigidBody("upperArm", joint, 0.0437, 0.0437, 0.0054, THIGH_MASS, comOffset); }
private RigidBodyBasics createHand(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("wristPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 4.0); return new RigidBody("hand", joint, 0.0437, 0.0437, 0.0054, FOOT_MASS, comOffset); }
private RigidBodyBasics createUpperArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("shoulderPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(Math.toRadians(20)); Vector3D comOffset = new Vector3D(0.0, 0.0, THIGH_LENGTH / 2.0); return new RigidBody("upperArm", joint, 0.0437, 0.0437, 0.0054, THIGH_MASS, comOffset); }
private RigidBodyBasics createLowerArm(String suffix, RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("elbow_y_" + suffix, parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 2.0); return new RigidBody("lowerArm" + suffix, joint, 0.0437, 0.0437, 0.0054, SHIN_MASS, comOffset); }
private RigidBodyBasics createUpperArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("shoulderPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, THIGH_LENGTH / 2.0); return new RigidBody("upperArm", joint, 0.0437, 0.0437, 0.0054, THIGH_MASS, comOffset); }
private RigidBodyBasics createHand(String suffix, RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("wristPitch_y_" + suffix, parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 4.0); return new RigidBody("hand" + suffix, joint, 0.0437, 0.0437, 0.0054, FOOT_MASS, comOffset); }
private RigidBodyBasics createHand(String suffix, RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("wristPitch_y_" + suffix, parentBody, new Vector3D(0.0, 0.0, SHIN_LENGTH), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 4.0); return new RigidBody("hand" + suffix, joint, 0.0437, 0.0437, 0.0054, FOOT_MASS, comOffset); }
private RigidBodyBasics createLowerArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("elbow_y", parentBody, new Vector3D(0.0, 0.0, THIGH_LENGTH), Y); joint.setQ(Math.toRadians(40)); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 2.0); return new RigidBody("lowerArm", joint, 0.0437, 0.0437, 0.0054, SHIN_MASS, comOffset); }
private RigidBodyBasics createLowerArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("elbow_y", parentBody, new Vector3D(0.0, 0.0, THIGH_LENGTH), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 2.0); return new RigidBody("lowerArm", joint, 0.0437, 0.0437, 0.0054, SHIN_MASS, comOffset); }
private RigidBodyBasics createUpperArm(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("shoulderPitch_y", parentBody, new Vector3D(0.0, 0.0, 0.0), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, THIGH_LENGTH / 2.0); return new RigidBody("upperArm", joint, 0.0437, 0.0437, 0.0054, THIGH_MASS, comOffset); }
private RigidBodyBasics createLowerArm(String suffix, RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("elbow_y_" + suffix, parentBody, new Vector3D(0.0, 0.0, THIGH_LENGTH), Y); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(0.0, 0.0, SHIN_LENGTH / 2.0); return new RigidBody("lowerArm" + suffix, joint, 0.0437, 0.0437, 0.0054, SHIN_MASS, comOffset); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeSubTreeMass() { Random random = new Random(100L); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBodyBasics elevator = new RigidBody("body", worldFrame); int numberOfJoints = 100; double addedMass = createRandomRigidBodyTreeAndReturnTotalMass(worldFrame, elevator, numberOfJoints, random); assertEquals(addedMass, TotalMassCalculator.computeSubTreeMass(elevator), 0.00001); }
private RigidBodyBasics copyLinkAsRigidBody(Link link, JointBasics currentInverseDynamicsJoint, String bodyName) { Vector3D comOffset = new Vector3D(); link.getComOffset(comOffset); Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); return new RigidBody(bodyName, currentInverseDynamicsJoint, momentOfInertia, link.getMass(), comOffset); }
private static RigidBodyBasics copyLinkAsRigidBody(Link link, JointBasics currentInverseDynamicsJoint, String bodyName) { Vector3D comOffset = new Vector3D(); link.getComOffset(comOffset); Matrix3D momentOfInertia = new Matrix3D(); link.getMomentOfInertia(momentOfInertia); return new RigidBody(bodyName, currentInverseDynamicsJoint, momentOfInertia, link.getMass(), comOffset); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddPrismaticJoint_String_RigidBody_Transform3D_Vector3d() { String jointName = "joint"; RigidBodyBasics parentBody = new RigidBody("body", ReferenceFrame.getWorldFrame()); RigidBodyTransform transformToParent = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); PrismaticJoint joint = new PrismaticJoint(jointName, parentBody, transformToParent, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddPrismaticJoint_String_RigidBody_Vector3d_Vector3d() { String jointName = "joint"; RigidBodyBasics parentBody = new RigidBody("body", ReferenceFrame.getWorldFrame()); Vector3D jointOffset = RandomGeometry.nextVector3D(random, 5.0); Vector3D jointAxis = RandomGeometry.nextVector3D(random, 5.0); PrismaticJoint joint = new PrismaticJoint(jointName, parentBody, jointOffset, jointAxis); assertEquals("Should be equal", jointName, joint.getName()); assertTrue(parentBody.equals(joint.getPredecessor())); }