public SinglePendulumRobot(String name, double initialQ, double initialQd) { super(name); pinJoint = new PinJoint(name + "Joint", new Vector3D(), this, axis); Link link = new Link(name + "Link"); link.setMass(mass); link.setMomentOfInertia(Ixx, 0.0, 0.0); link.setComOffset(0.0, 0.0, -0.5 * length); pinJoint.setLink(link); pinJoint.setDamping(damping); addRootJoint(pinJoint); pinJoint.setQ(initialQ); pinJoint.setQd(initialQd); }
public TwoLinkRobotForTesting() { super("TwoLink"); this.gravity = 9.81; this.setGravity(0.0, 0.0, -gravity); upperJoint = new PinJoint("upper", new Vector3D(0.0, 0.0, 4.0 * linkLength), this, Axis.Y); //upperJoint.setCartesianPosition(0.0, activeLegLength + passiveLegLength + footLength + 0.13); Link upperArmLink = upperArm(); upperJoint.setLink(upperArmLink); //upperJoint.setDamping(1.0); upperJoint.setLimitStops(-2.0, 5.0, 2.0, 1000.0); upperJoint.setVelocityLimits(7.0, 1000.0); upperJoint.setDynamic(true); this.addRootJoint(upperJoint); elbowJoint = new PinJoint("elbow", new Vector3D(0.0, 0.0, -linkLength), this, Axis.Y); upperJoint.addJoint(elbowJoint); Link lowerArmLink = lowerArm(); elbowJoint.setLink(lowerArmLink); //elbowJoint.setDamping(1.0); elbowJoint.setLimitStops(-4.0, 3.0, 2.0, 1.0); elbowJoint.setVelocityLimits(4.0, 1.0); }
PinJointRobotSensor(PinJoint joint) { q = joint.getQYoVariable(); qd = joint.getQDYoVariable(); qdd = joint.getQDDYoVariable(); tau_actual = joint.getTauYoVariable(); }
private static PinJoint nextPinJoint(long seed, Robot rob) { Random random = new Random(seed); String jname = "randomPinJoint" + random.nextInt(); Vector3DReadOnly offset = EuclidCoreRandomTools.nextVector3D(random); Axis jaxis = Axis.values[random.nextInt(Axis.values.length)]; PinJoint pinJoint = new PinJoint(jname, offset, rob, jaxis); pinJoint.setQ(random.nextDouble()); pinJoint.setQd(random.nextDouble()); return pinJoint; }
PinJoint pin1 = new PinJoint("pin1", offset, robot1, Axis.Y); root1.addJoint(pin1); Link link21 = link21(random, l2, r2); pin1.setLink(link21); root2.setLink(link22); Vector3D jointAxis = new Vector3D(); pin1.getJointAxis(jointAxis); PinJoint pin2 = new PinJoint("pin2", new Vector3D(), robot2, jointAxis); root2.addJoint(pin2); Link link12 = link12(link11, pin1); pin2.setLink(link12); pin1.setQ(random.nextDouble()); pin2.setQ(-pin1.getQYoVariable().getDoubleValue()); pin1.setQd(10.0); pin2.setQd(-pin1.getQDYoVariable().getDoubleValue()); Vector3D angularVelocityInBody = new Vector3D(0.0, pin1.getQDYoVariable().getDoubleValue(), 0.0); root2.setAngularVelocityInBody(angularVelocityInBody); pin1.setTau(random.nextDouble()); pin2.setTau(-pin1.getTauYoVariable().getDoubleValue()); pin1.getTransformToWorld(pin1ToWorld); root2.setRotationAndTranslation(pin1ToWorld); root2.setPosition(root2.getQx().getDoubleValue(), root2.getQy().getDoubleValue(), root2.getQz().getDoubleValue()); assertEquals(pin1.getQDDYoVariable().getDoubleValue(), pin1.getQDDYoVariable().getDoubleValue(), 1e-8);
PinJoint joint1 = new PinJoint("joint1", jointOffset1, robotOne, Axis.X); Link link1 = new Link("link1"); link1.setMass(mass1); link1.setMomentOfInertia(momentOfInertia1); link1.setComOffset(comOffset1); joint1.setLink(link1); PinJoint joint2 = new PinJoint("joint2", jointOffset2, robotOne, Axis.Y); Link link2 = new Link("link2"); link2.setMass(mass2); link2.setMomentOfInertia(momentOfInertia2); link2.setComOffset(comOffset2); joint2.setLink(link2); PinJoint joint3 = new PinJoint("joint3", jointOffset3, robotOne, Axis.Y); Link link3 = new Link("link3"); link3.setMass(mass3); link3.setMomentOfInertia(momentOfInertia3); link3.setComOffset(comOffset3); joint3.setLink(link3); joint1.addJoint(joint2); joint2.addJoint(joint3); joint1.changeOffsetVector(jointOffset1); joint2.changeOffsetVector(jointOffset2); Link newLink2 = new Link(joint2.getLink().getName());
valvePinJoint = new PinJoint("valvePinJoint", valvePositionInWorld, this, jointAxisVector); valvePinJoint.setLimitStops(0.0, valveNumberOfPossibleTurns * 2 * Math.PI, 1000, 100); valvePinJoint.setDamping(valveDamping.getDoubleValue()); valvePinJoint.setLink(valveLink); this.addRootJoint(valvePinJoint); valvePinJoint.getQYoVariable().addVariableChangedListener(new VariableChangedListener()
PinJoint l_hip_yaw = new PinJoint("l_leg_hpz", leftHipYawOffset, robotLeg, Axis.Z); PinJoint r_hip_yaw = new PinJoint("r_leg_hpz", rightHipYawOffset, robotLeg, Axis.Z); l_hip_yaw.setQ(random.nextDouble()); r_hip_yaw.setQ(random.nextDouble()); l_hip_yaw.setLink(l_hip_differential); r_hip_yaw.setLink(r_hip_differential); floatingJoint.addJoint(l_hip_yaw); floatingJoint.addJoint(r_hip_yaw); l_leg_hpz.setQ(l_hip_yaw.getQYoVariable().getDoubleValue()); r_leg_hpz.setQ(r_hip_yaw.getQYoVariable().getDoubleValue()); RigidBodyBasics leftHipDifferentialBody = copyLinkAsRigidBody(l_hip_differential, l_leg_hpz, "l_hip_differential"); RigidBodyBasics rightHipDifferentialBody = copyLinkAsRigidBody(r_hip_differential, r_leg_hpz, "r_hip_differential"); PinJoint l_hip_roll = new PinJoint("l_leg_hpx", leftHipRollOffset, robotLeg, Axis.X); PinJoint r_hip_roll = new PinJoint("r_leg_hpx", rightHipRollOffset, robotLeg, Axis.X); l_hip_roll.setQ(random.nextDouble()); r_hip_roll.setQ(random.nextDouble()); l_hip_roll.setLink(l_hip_differential2); r_hip_roll.setLink(r_hip_differential2); l_hip_yaw.addJoint(l_hip_roll); r_hip_yaw.addJoint(r_hip_roll); l_leg_hpx.setQ(l_hip_roll.getQYoVariable().getDoubleValue()); r_leg_hpx.setQ(r_hip_roll.getQYoVariable().getDoubleValue()); RigidBodyBasics leftHipDifferentialBody2 = copyLinkAsRigidBody(l_hip_differential2, l_leg_hpx, "l_hip_differential"); RigidBodyBasics rightHipDifferentialBody2 = copyLinkAsRigidBody(r_hip_differential2, r_leg_hpx, "r_hip_differential");
PinJoint pinJoint1 = new PinJoint("pinJoint1", new Vector3d(), this, Axis.X); Link armLink1 = new Link("armLink1"); armLink1.setMassAndRadiiOfGyration(0.3, 0.1, 0.1, 0.1); pinJoint1.setLink(armLink1); pinJoint1.addKinematicPoint(kinematicPoint1); IMUMount imuMount1 = new IMUMount("imuMount1", imu1Offset, this); pinJoint1.addIMUMount(imuMount1); PinJoint pinJoint2 = new PinJoint("pinJoint2", new Vector3d(0.0, 0.0, 1.0), this, Axis.Z); Link armLink2 = new Link("armLink2"); armLink2.setMassAndRadiiOfGyration(0.2, 0.1, 0.1, 0.1); armLink2Graphics.addCylinder(1.0, 0.02, YoAppearance.Blue()); armLink2.setLinkGraphics(armLink2Graphics); pinJoint2.setLink(armLink2); pinJoint2.addKinematicPoint(kinematicPoint2); IMUMount imuMount2 = new IMUMount("imuMount2", imu2Offset, this); pinJoint2.addIMUMount(imuMount2); pinJoint2.addKinematicPoint(positionAndVelocityPoint2); pinJoint1.addJoint(pinJoint2); pinJoint1.setQ(1.2); pinJoint2.setQ(0.8);
private void createDoor(Vector3D positionInWorld) { // creating the pinJoint, i.e. door hinge doorHingePinJoint = new PinJoint("doorHingePinJoint", positionInWorld, this, Axis.Z); // door link doorLink = new Link("doorLink"); doorLink.setMass(mass); doorLink.setComOffset(0.5*widthX, 0.5*depthY, 0.5*heightZ); inertiaMatrix = RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidBox(widthX, depthY, heightZ, mass); doorLink.setMomentOfInertia(inertiaMatrix); doorHingePinJoint.setLink(doorLink); this.addRootJoint(doorHingePinJoint); }
double jointAcceleration = useRandomAcceleration ? random.nextDouble() : 0.0; PinJoint currentJoint = new PinJoint("joint" + i, jointOffset, robot, jointAxis); currentJoint.setInitialState(jointPosition, jointVelocity); RigidBodyBasics inverseDynamicsParentBody; if (potentialParentJoints.isEmpty()) potentialParentJoints.get(parentIndex).addJoint(currentJoint); RevoluteJoint inverseDynamicsParentJoint = potentialInverseDynamicsParentJoints.get(parentIndex); inverseDynamicsParentBody = inverseDynamicsParentJoint.getSuccessor(); currentBody.setMass(mass); currentBody.setMomentOfInertia(momentOfInertia); currentJoint.setLink(currentBody);
private Robot createSimpleRobotOne(String name) { Robot robot = new Robot(name); PinJoint joint1 = new PinJoint("joint", new Vector3D(0.0, 0.0, 0.0), robot, Axis.Y); Link link1 = link1(); joint1.setLink(link1); robot.addRootJoint(joint1); joint1.setInitialState(0.11, 0.22); joint1.setTau(33.3); return robot; }
private void createHandle() { // create handle handlePinJoint = new PinJoint("handlePinJoint", new Vector3D(handleOffset.getX(), 0.0, handleOffset.getY()), this, Axis.Y); // handle link handleLink = new Link("handleHorizontalLink"); handleLink.setMass(0.2); handleLink.setMomentOfInertia(0.1, 0.1, 0.1); // TODO handlePinJoint.setLink(handleLink); doorHingePinJoint.addJoint(handlePinJoint); handlePinJoint.setKd(2.0); handlePinJoint.setKp(0.5); }
pinJointOne.addJointWrenchSensor(jointWrenchSensorOne); assertTrue(jointWrenchSensorOne == pinJointOne.getJointWrenchSensor()); robot.addRootJoint(pinJointOne); pinJointTwo.addJointWrenchSensor(jointWrenchSensorTwo); assertTrue(jointWrenchSensorTwo == pinJointTwo.getJointWrenchSensor()); pinJointOne.addJoint(pinJointTwo); pinJointOne.setQ(RandomNumbers.nextDouble(random, -Math.PI, Math.PI)); pinJointTwo.setQ(RandomNumbers.nextDouble(random, -Math.PI, Math.PI)); pinJointOne.setQd(RandomNumbers.nextDouble(random, -1.0, 1.0)); pinJointTwo.setQd(RandomNumbers.nextDouble(random, -1.0, 1.0)); pinJointOne.setTau(RandomNumbers.nextDouble(random, -1.0, 1.0)); pinJointTwo.setTau(RandomNumbers.nextDouble(random, -1.0, 1.0)); assertEquals(pinJointOne.getTauYoVariable().getDoubleValue(), -jointTorque.getY(), 1e-7); assertEquals(pinJointTwo.getTauYoVariable().getDoubleValue(), -jointTorque.getX(), 1e-7);
@Test// timeout=300000 public void testFloatingJointAndPinJointWithMassiveBody() throws UnreasonableAccelerationException { Random random = new Random(1659L); Robot robot = new Robot("r1"); robot.setGravity(0.0); FloatingJoint root1 = new FloatingJoint("root1", new Vector3D(), robot); robot.addRootJoint(root1); Link floatingBody = new Link("floatingBody"); floatingBody.setMass(random.nextDouble()); floatingBody.setComOffset(random.nextDouble(), random.nextDouble(), random.nextDouble()); floatingBody.setMomentOfInertia(getRotationalInertiaMatrixOfSolidEllipsoid(floatingBody.getMass(), random.nextDouble(), random.nextDouble(), random.nextDouble())); root1.setLink(floatingBody); Vector3D offset = EuclidCoreRandomTools.nextVector3D(random); PinJoint pin1 = new PinJoint("pin1", offset, robot, EuclidCoreRandomTools.nextVector3D(random)); pin1.setLink(massiveLink()); root1.addJoint(pin1); pin1.setTau(random.nextDouble()); robot.doDynamicsButDoNotIntegrate(); double scalarInertiaAboutJointAxis = computeScalarInertiaAroundJointAxis(floatingBody, pin1); double torqueFromDynamics = scalarInertiaAboutJointAxis * pin1.getQDDYoVariable().getDoubleValue(); assertEquals(pin1.getTauYoVariable().getDoubleValue(), torqueFromDynamics, pin1.getTauYoVariable().getDoubleValue() * 1e-3); }
public DoublePendulum() { super("DoublePendulum"); j1 = new PinJoint("j1", new Vector3D(0, 0, 2), this, Axis.X); Link l1 = new Link("l1"); l1.setComOffset(0, 0, 0.5); l1.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3); l1.addEllipsoidFromMassProperties(YoAppearance.Pink()); j1.setLink(l1); j2 = new PinJoint("j2", new Vector3D(0.0, 0.0, 1.0), this, Axis.X); Link l2 = new Link("l2"); l2.setComOffset(0, 0, 0.5); l2.setMassAndRadiiOfGyration(1.0, 0.05, 0.05, 0.3); l2.addEllipsoidFromMassProperties(YoAppearance.Purple()); j2.setLink(l2); j1.addJoint(j2); addRootJoint(j1); }
public void setSteeringAngleInDegrees(double steeringAngle) { steeringWheelPinJoint.setQ(steeringAngle); } }
private void writeJointDataToWorkbook(WritableWorkbook workbook) { WritableSheet jointDataSheet = workbook.createSheet("Joint Data", workbook.getNumberOfSheets()); int column = 0; writeJointDataColumn(jointDataSheet, column++, dataBuffer.getEntry("t"), true); for (PinJoint joint : pinJoints) { DataBufferEntry position = dataBuffer.getEntry(joint.getQYoVariable()); DataBufferEntry speed = dataBuffer.getEntry(joint.getQDYoVariable()); DataBufferEntry torque = dataBuffer.getEntry(joint.getTauYoVariable()); writeJointDataColumn(jointDataSheet, column++, position, false); writeJointDataColumn(jointDataSheet, column++, speed, false); writeJointDataColumn(jointDataSheet, column++, speed, true); writeJointDataColumn(jointDataSheet, column++, torque, false); writeJointDataColumn(jointDataSheet, column++, torque, true); writeMechanicalPowerJointDataColumn(jointDataSheet, column++, speed, torque, joint.getName()); } }
robot1.addRootJoint(floatingJoint1); floatingJoint1.setLink(randomBodyNoYCoMOffset(random)); PinJoint pin1 = new PinJoint("pin1", offset, robot1, Axis.Y); floatingJoint1.addJoint(pin1); pin1.setLink(randomBodyNoYCoMOffset(random)); robot2.addRootJoint(floatingJoint2); floatingJoint2.setLink(new Link(floatingJoint1.getLink())); PinJoint pin2 = new PinJoint("pin2", offset, robot2, Axis.Y); floatingJoint2.addJoint(pin2); pin2.setLink(new Link(pin1.getLink()));
addRootJoint(rootJoint); lidarJoint = new PinJoint("lidarJoint", new Vector3D(), this, Axis.X); lidarSensorDescription.setHeightPitchLimits(lidarScanParameters.heightPitchMin, lidarScanParameters.heightPitchMax); LidarMount lidarMount = new LidarMount(lidarSensorDescription); lidarJoint.addLidarMount(lidarMount); lidarJoint.setLink(link); rootJoint.addJoint(lidarJoint);