/** * If part of a kinematic loop, this should only be called by that loop's calculator */ @Override public void setQ(double q) { super.setQ(q); }
@Before public void setUp() throws Exception { buildMechanismAndJacobians(); // joint positions joint1.setQ(random.nextDouble()); joint2.setQ(random.nextDouble()); joint3.setQ(random.nextDouble()); }
/** {@inheritDoc} */ @Override public void setJointOrientation(Orientation3DReadOnly jointOrientation) { jointOrientation.getRotationVector(rotationVector); setQ(rotationVector.dot(jointAxis)); }
public void generateRandomConfiguration() { for (RobotArmJointParameters armJoint : allJointNames) { double q = RandomNumbers.nextDouble(random, armJoint.getJointLowerLimit(), armJoint.getJointUpperLimit()); robotArmRevoluteJoints.get(armJoint).setQ(q); } }
private RigidBodyBasics createDifferential(String name, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis) { String jointName; if (jointAxis == X) jointName = name + "_x"; else if (jointAxis == Y) jointName = name + "_y"; else jointName = name + "_z"; RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(); return new RigidBody(jointName, joint, 0.005, 0.005, 0.005, 0.1, comOffset); }
private RigidBodyBasics createDifferential(String name, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis) { String jointName; if (jointAxis == X) jointName = name + "_x"; else if (jointAxis == Y) jointName = name + "_y"; else jointName = name + "_z"; RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(); return new RigidBody(name, joint, 0.005, 0.005, 0.005, 0.1, comOffset); }
private void setRandomPositions(Random random, List<RevoluteJoint> revoluteJoints, double deltaThetaMax) { for (RevoluteJoint revoluteJoint : revoluteJoints) { // revoluteJoint.setQ(revoluteJoint.getQ() + RandomTools.generateRandomDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.setQ(RandomNumbers.nextDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.getFrameAfterJoint().update(); } } }
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 createHand(RigidBodyBasics parentBody) { RevoluteJoint joint = new RevoluteJoint("wristPitch_y", parentBody, new Vector3D(0.0, 0.0, SHIN_LENGTH), Y); joint.setQ(Math.toRadians(30)); 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); }
private void setRandomPositions(Random random, List<RevoluteJoint> revoluteJoints, double deltaThetaMax) { for (RevoluteJoint revoluteJoint : revoluteJoints) { revoluteJoint.setQ(revoluteJoint.getQ() + RandomNumbers.nextDouble(random, -deltaThetaMax, deltaThetaMax)); revoluteJoint.getFrameAfterJoint().update(); } } }