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; }
double jointAcceleration = useRandomAcceleration ? random.nextDouble() : 0.0; PinJoint currentJoint = new PinJoint("joint" + i, jointOffset, robot, jointAxis); currentJoint.setInitialState(jointPosition, jointVelocity); RigidBodyBasics inverseDynamicsParentBody;
private Robot createRobot() { Robot robot = new Robot("RandomRobot"); PinJoint pinJoint = new PinJoint("TestPinJoint", new Vector3D(), robot, new Vector3D(0, 0, 1)); robot.addRootJoint(pinJoint); return robot; } }
@Test// timeout=300000 public void testGetAndSetParentJoint() { PinJoint joint = new PinJoint("joint", new Vector3D(0.0, 0.0, 0.0), robot, Axis.X); robot.addRootJoint(joint); kinematicPoint.setParentJoint(joint); assertTrue(joint == kinematicPoint.getParentJoint()); }
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); }
private PinJoint createPinJointWithHangingMass(String name, double mass, Axis axis, Robot robot) { PinJoint pinJoint = new PinJoint(name, new Vector3D(), robot, axis); Vector3D comOffset = new Vector3D(0.0, 0.0, -1.0); Link linkOne = new Link("link"); linkOne.setMass(mass); linkOne.setComOffset(comOffset); linkOne.setMassAndRadiiOfGyration(mass, 0.1, 0.1, 0.1); pinJoint.setLink(linkOne); return pinJoint; }
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 Robot createSimpleRobotTwo(String name) { Robot robot = new Robot(name); PinJoint joint2 = new PinJoint("joint", new Vector3D(0.0, 0.0, 0.0), robot, Axis.Y); Link link2 = link2(); joint2.setLink(link2); robot.addRootJoint(joint2); joint2.setInitialState(0.11, 0.22); joint2.setTau(33.3); return robot; }
@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); }
PinJoint currentJoint = new PinJoint("joint" + i, jointOffset, robot, jointAxis); currentJoint.setInitialState(jointPosition, jointVelocity); if (previousJoint == null)
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); }
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); }
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); }
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); }
PinJoint pinJoint = new PinJoint(armJoint.getJointName(false), armJoint.getJointOffset(), this, armJoint.getJointAxis()); pinJoint.setLimitStops(armJoint.getJointLowerLimit(), armJoint.getJointUpperLimit(), 100.0, 10.0); pinJoint.setDynamic(false);
PinJoint pinJoint = new PinJoint(armJoint.getJointName(false), armJoint.getJointOffset(), this, armJoint.getJointAxis()); pinJoint.setLimitStops(armJoint.getJointLowerLimit(), armJoint.getJointUpperLimit(), 100.0, 10.0); pinJoint.setDynamic(false);
public DoublePendulumRobot(String name, double initialQFirstJoint, double initialQdFirstJoint, double initialQSecondJoint, double initialQdSecondJoint) { super(name); pinJoint1 = new PinJoint(name + "Joint1", new Vector3D(), this, axis); Link link1 = new Link(name + "Link1"); link1.setMass(mass1); link1.setMomentOfInertia(Ixx1CoM, 0.0, 0.0); link1.setComOffset(0.0, 0.0, - lengthCoM1); pinJoint1.setLink(link1); pinJoint1.setDamping(damping1); pinJoint2 = new PinJoint(name + "Joint2", new Vector3D(0.0, 0.0, -length1), this, axis); Link link2 = new Link(name + "Link2"); link2.setMass(mass2); link2.setMomentOfInertia(Ixx2CoM, 0.0, 0.0); link2.setComOffset(0.0, 0.0, - lengthCoM2); pinJoint2.setLink(link2); pinJoint2.setDamping(damping2); pinJoint1.addJoint(pinJoint2); pinJoint1.setQ(initialQFirstJoint); pinJoint1.setQd(initialQdFirstJoint); pinJoint2.setQ(initialQSecondJoint); pinJoint2.setQd(initialQdSecondJoint); addRootJoint(pinJoint1); }
public CartPoleRobot(String name, double initialCartVelocity, double initialPoleAngle, double initialPoleAngularVelocity) { super(name); sliderJoint = new SliderJoint(name + "CartJoint", new Vector3D(), this, Axis.X); sliderJoint.setDamping(cartDamping); Link cartLink = new Link(name + "CartLink"); cartLink.setMass(cartMass); sliderJoint.setLink(cartLink); pinJoint = new PinJoint(name + "PoleJoint", new Vector3D(), this, Axis.Y); pinJoint.setDamping(poleDamping); Link poleLink = new Link(name + "PoleLink"); poleLink.setMass(poleMass); poleLink.setComOffset(0.0, 0.0, - poleLength); pinJoint.setLink(poleLink); sliderJoint.setQd(initialCartVelocity); pinJoint.setQ(initialPoleAngle); pinJoint.setQd(initialPoleAngularVelocity); sliderJoint.addJoint(pinJoint); addRootJoint(sliderJoint); }
pinJoint = new PinJoint(name + "PinJoint", offset, this, axis);
addRootJoint(rootJoint); lidarJoint = new PinJoint("lidarJoint", new Vector3D(), this, Axis.X);