public void attachFaceCube() { Link faceLink = new Link("FaceLink"); Graphics3DObject faceGraphics = new Graphics3DObject(); faceGraphics.scale(new Vector3d(3.0, 3.0, 0.1)); faceGraphics.addModelFile("models/GFE/ihmc/face_cube.dae"); faceLink.setLinkGraphics(faceGraphics); FloatingJoint faceJoint = new FloatingJoint("FaceJoint", "faceJoint" , new Vector3d(), this); faceJoint.setRotationAndTranslation(new RigidBodyTransform(new AxisAngle4d(new Vector3d(0.0, 1.0, 0.0), - Math.PI / 2.0), new Vector3d(1.1, 0.0, 1.3))); faceJoint.setLink(faceLink); faceJoint.setDynamic(false); floatingJoint.addJoint(faceJoint); }
public VisualizerRobot(RobotDescription robotDescription) { super(robotDescription, false, false); this.reducedRegistry = new YoVariableRegistry(robotDescription.getName()); reducedRegistry.registerVariable(getRootJoint().getQx()); reducedRegistry.registerVariable(getRootJoint().getQy()); reducedRegistry.registerVariable(getRootJoint().getQz()); reducedRegistry.registerVariable(getRootJoint().getQdx()); reducedRegistry.registerVariable(getRootJoint().getQdy()); reducedRegistry.registerVariable(getRootJoint().getQdz()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQs()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQx()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQy()); reducedRegistry.registerVariable(getRootJoint().getQuaternionQz()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityX()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityY()); reducedRegistry.registerVariable(getRootJoint().getAngularVelocityZ()); for(OneDegreeOfFreedomJoint joint : getOneDegreeOfFreedomJoints()) { reducedRegistry.registerVariable(joint.getQYoVariable()); reducedRegistry.registerVariable(joint.getQDYoVariable()); } }
public void get(FloatingJoint joint) { rotationMatrix.set(rotation); joint.setRotation(rotationMatrix); joint.setPosition(translation); twist.getAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); twist.getLinearPart(tempVector); joint.setVelocity(tempVector); }
FloatingJointRobotSensor(FloatingJoint joint) { q_x = joint.getQx(); q_y = joint.getQy(); q_z = joint.getQz(); qd_x = joint.getQdx(); qd_y = joint.getQdy(); qd_z = joint.getQdz(); q_qs = joint.getQuaternionQs(); q_qx = joint.getQuaternionQx(); q_qy = joint.getQuaternionQy(); q_qz = joint.getQuaternionQz(); qd_wx = joint.getAngularVelocityX(); qd_wy = joint.getAngularVelocityY(); qd_wz = joint.getAngularVelocityZ(); qdd_x = joint.getQddx(); qdd_y = joint.getQddy(); qdd_z = joint.getQddz(); qdd_wx = joint.getAngularAccelerationX(); qdd_wy = joint.getAngularAccelerationY(); qdd_wz = joint.getAngularAccelerationZ(); }
private void setRandomVelocity(FloatingJoint floatingJoint, SixDoFJoint sixDoFJoint) { Vector3D linearVelocity = RandomGeometry.nextVector3D(random, 1.0); Vector3D angularVelocity = RandomGeometry.nextVector3D(random, 1.0); floatingJoint.setVelocity(linearVelocity); floatingJoint.setAngularVelocityInBody(angularVelocity); ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint(); floatingJoint.getVelocity(linearVelocityFrameVector); linearVelocityFrameVector.changeFrame(bodyFrame); floatingJoint.getAngularVelocity(angularVelocityFrameVector, bodyFrame); Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, angularVelocityFrameVector, linearVelocityFrameVector); sixDoFJoint.setJointTwist(bodyTwist); }
FloatingJoint floatingJointA = new FloatingJoint("unpinning", new Vector3D(), robotA); floatingJointA.setPinned(true); long rootLinkSeed = random.nextLong(); floatingJointA.setLink(nextLink(rootLinkSeed)); robotA.addRootJoint(floatingJointA); FloatingJoint floatingJointB = new FloatingJoint("unpinning", new Vector3D(), robotB); floatingJointB.setLink(nextLink(rootLinkSeed)); robotB.addRootJoint(floatingJointB); floatingJointA.setVelocity(EuclidCoreRandomTools.nextVector3D(random)); floatingJointA.setAngularVelocityInBody(EuclidCoreRandomTools.nextVector3D(random)); floatingJointA.setAcceleration(EuclidCoreRandomTools.nextVector3D(random)); floatingJointA.setAngularAccelerationInBody(EuclidCoreRandomTools.nextVector3D(random)); floatingJointB.getQx().set(floatingJointA.getQx().getValue()); floatingJointB.getQy().set(floatingJointA.getQy().getValue()); floatingJointB.getQz().set(floatingJointA.getQz().getValue()); floatingJointB.getQuaternionQs().set(floatingJointA.getQuaternionQs().getValue()); floatingJointB.getQuaternionQx().set(floatingJointA.getQuaternionQx().getValue()); floatingJointB.getQuaternionQy().set(floatingJointA.getQuaternionQy().getValue()); floatingJointB.getQuaternionQz().set(floatingJointA.getQuaternionQz().getValue()); floatingJointA.setPinned(false); assertEquals(floatingJointB.getQx().getValue(), floatingJointA.getQx().getValue(), EPSILON); assertEquals(floatingJointB.getQy().getValue(), floatingJointA.getQy().getValue(), EPSILON); assertEquals(floatingJointB.getQz().getValue(), floatingJointA.getQz().getValue(), EPSILON); EuclidCoreTestTools.assertQuaternionEquals(floatingJointB.getQuaternion(), floatingJointA.getQuaternion(), EPSILON);
q_x = rootJoint.getQx(); q_y = rootJoint.getQy(); q_z = rootJoint.getQz(); q_qs = rootJoint.getQuaternionQs(); q_qx = rootJoint.getQuaternionQx(); q_qy = rootJoint.getQuaternionQy(); q_qz = rootJoint.getQuaternionQz();
FloatingJoint root1 = new FloatingJoint("root1", new Vector3D(), robot1); robot1.addRootJoint(root1); Link link11 = link11(random, l1, r1); root1.setLink(link11); root1.addJoint(pin1); Link link21 = link21(random, l2, r2); pin1.setLink(link21); FloatingJoint root2 = new FloatingJoint("root2", new Vector3D(), robot2); robot2.addRootJoint(root2); Link link22 = link22(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); pin2.setQd(-pin1.getQDYoVariable().getDoubleValue()); Vector3D angularVelocityInBody = new Vector3D(0.0, pin1.getQDYoVariable().getDoubleValue(), 0.0); root2.setAngularVelocityInBody(angularVelocityInBody); RigidBodyTransform pin1ToWorld = new RigidBodyTransform(); pin1.getTransformToWorld(pin1ToWorld); root2.setRotationAndTranslation(pin1ToWorld); root2.setPosition(root2.getQx().getDoubleValue(), root2.getQy().getDoubleValue(), root2.getQz().getDoubleValue()); robot2.update();
@Test// timeout=300000 public void testCalculateAngularMomentum() { double epsilon = 1e-7; Robot robot1 = new Robot("r1"); FloatingJoint floatingJoint1 = new FloatingJoint("joint1", new Vector3D(),robot1); robot1.addRootJoint(floatingJoint1); Link onlyLink=new Link("SphericalLink"); onlyLink.setComOffset(new Vector3D(0.0, 0.0, 0.0)); onlyLink.setMass(1.0); onlyLink.setMomentOfInertia(1.0, 1.0, 1.0); floatingJoint1.setLink(onlyLink); floatingJoint1.setPosition(new Point3D(1.0,1.0,1.0)); floatingJoint1.setAngularVelocityInBody(new Vector3D(1.0,0.0,0.0)); floatingJoint1.setVelocity(-1.0, 0.0, 0.0); Vector3D angularMomentumReturned = new Vector3D(); robot1.computeAngularMomentum(angularMomentumReturned); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(0.0, 0.0, 0.0), angularMomentumReturned, epsilon); robot1.update(); robot1.computeAngularMomentum(angularMomentumReturned); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(0.0, 0.0, 0.0), angularMomentumReturned, epsilon); robot1.updateVelocities(); robot1.computeAngularMomentum(angularMomentumReturned); //System.out.printf("Momentum <%+3.4f,%+3.4f,%+3.4f>", angularMomentumReturned.x,angularMomentumReturned.y,angularMomentumReturned.z); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(1.0, -1.0, 1.0), angularMomentumReturned, epsilon); floatingJoint1.setPosition(new Vector3D()); }
public RectangleRobot() { super("RectangleRobot"); baseJoint = new FloatingJoint("base", new Vector3D(0.0, 0.0, 0.0), this); Link link = base("base"); baseJoint.setLink(link); this.addRootJoint(baseJoint); }
rootJoint = new FloatingJoint("root", new Vector3d(), this); rootJoint.addExternalForcePoint(externalForcePoint); bodyLinkGraphics.addCube(0.1, 0.2, 0.3, YoAppearance.Red()); bodyLink.setLinkGraphics(bodyLinkGraphics); rootJoint.setLink(bodyLink); rootJoint.addKinematicPoint(kinematicPoint0); rootJoint.addIMUMount(imuMount0); rootJoint.addKinematicPoint(positionAndVelocityPoint0); pinJoint1.addIMUMount(imuMount1); rootJoint.addJoint(pinJoint1); rootJoint.setPosition(new Point3d(0.0, 0.0, 0.4)); Matrix3d rotationMatrix = new Matrix3d(); rotationMatrix.rotX(0.6); rootJoint.setRotation(rotationMatrix); rootJoint.setAngularVelocityInBody(new Vector3d(0.1, 0.2, 0.07));
FloatingJoint floatingJoint = new FloatingJoint("base", new Vector3D(), robot); link.setMomentOfInertia(Ixx, Iyy, Izz); link.setComOffset(new Vector3D()); floatingJoint.setLink(link); floatingJoint.addExternalForcePoint(externalForcePoint); Vector3D initialVelocity = new Vector3D(0.0, 0.0, -downwardVelocity); floatingJoint.setVelocity(initialVelocity); floatingJoint.setAngularVelocityInBody(new Vector3D(0.0, 0.0, 0.0)); robot.doDynamicsButDoNotIntegrate(); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(0.0, 0.0, mass * deltaVelocity), impulseInWorldToPack, 1e-7); Vector3D velocity = new Vector3D(); floatingJoint.getVelocity(velocity); floatingJoint.setVelocity(initialVelocity); floatingJoint.setAngularVelocityInBody(new Vector3D(0.13, 1.0, 0.11)); robot.update(); robot.updateVelocities();
@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); }
FloatingJoint floatingJoint1 = new FloatingJoint("joint1", new Vector3D(), robot1); robot1.addRootJoint(floatingJoint1); floatingJoint1.setLink(randomBodyNoYCoMOffset(random)); PinJoint pin1 = new PinJoint("pin1", offset, robot1, Axis.Y); floatingJoint1.addJoint(pin1); pin1.setLink(randomBodyNoYCoMOffset(random)); FloatingPlanarJoint floatingJoint2 = new FloatingPlanarJoint("joint2", robot2); robot2.addRootJoint(floatingJoint2); floatingJoint2.setLink(new Link(floatingJoint1.getLink())); PinJoint pin2 = new PinJoint("pin2", offset, robot2, Axis.Y); floatingJoint2.addJoint(pin2); assertEquals(floatingJoint1.getQddx().getDoubleValue(), floatingJoint2.getQdd_t1().getDoubleValue(), epsilon); assertEquals(floatingJoint1.getQddy().getDoubleValue(), 0.0, epsilon); assertEquals(floatingJoint1.getQddz().getDoubleValue(), floatingJoint2.getQdd_t2().getDoubleValue(), epsilon); assertEquals(floatingJoint1.getAngularAccelerationX().getDoubleValue(), 0.0, epsilon); assertEquals(floatingJoint1.getAngularAccelerationY().getDoubleValue(), floatingJoint2.getQdd_rot().getDoubleValue(), epsilon); assertEquals(floatingJoint1.getAngularAccelerationZ().getDoubleValue(), 0.0, epsilon);
robot.setGravity(0.0); FloatingJoint root1 = new FloatingJoint("root1", new Vector3D(), robot); robot.addRootJoint(root1); root1.setLink(floatingBody); Vector3D comOffset = floatingBody.getComOffset(); root1.addExternalForcePoint(externalForcePoint); Vector3D force = new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); externalForcePoint.setForce(force); root1.getTransformToWorld(transformToWorld); RigidBodyTransform transformToBody = new RigidBodyTransform(); transformToBody.setAndInvert(transformToWorld); temp1.set(root1.getAngularAccelerationInBody()); temp2.set(root1.getAngularVelocityInBody()); temp2.cross(root1.getAngularVelocityInBody(), temp2); // omega x J omega
FloatingJoint rootJoint = new FloatingJoint("root", new Vector3D(), robot); rootJoint.setLink(link); robot.addRootJoint(rootJoint); rootJoint.setAngularVelocityInBody(rotationAxis);
public ContactableCylinderRobot(String name, RigidBodyTransform rootJointTransform, double radius, double height, double mass, String modelName) { super(name); rootJointTransformToWorld = rootJointTransform; afterRootJointFrame = new ReferenceFrame("rootJointFrame", worldFrame) { @Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { floatingJoint.getTransformToWorld(transformToParent); } }; frameCylinder = new FrameCylinder3d(afterRootJointFrame, height, radius); link = new Link(name + "Link"); link.setMass(mass); link.setComOffset(new Vector3D(0.0, 0.0, height / 3.0)); Matrix3D inertia = RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidCylinder(mass, radius, height, Axis.Z); link.setMomentOfInertia(inertia); linkGraphics = new Graphics3DObject(); //linkGraphics.addCoordinateSystem(0.2); linkGraphics.addCylinder(frameCylinder.getHeight(), frameCylinder.getRadius(), YoAppearance.Transparent()); if(modelName!=null) linkGraphics.addModelFile(modelName); link.setLinkGraphics(linkGraphics); floatingJoint = new FloatingJoint(name + "Base", name, new Vector3D(), this); floatingJoint.setRotationAndTranslation(rootJointTransform); floatingJoint.setLink(link); this.addRootJoint(floatingJoint); }
FloatingJoint jointOne = new FloatingJoint("one", "cubeOne", new Vector3D(), robot); Link linkOne = new Link("CubeOne"); FloatingJoint jointTwo = new FloatingJoint("two", "cubeTwo", new Vector3D(), robot); Link linkTwo = new Link("CubeTwo"); jointOne.setLink(linkOne); jointTwo.setLink(linkTwo); jointOne.setPosition(x, y, z); robot.update();
public void setRobotStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity) { FloatingJoint rootJoint = robot.getRootJoint(); rootJoint.setVelocity(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setAngularVelocityInBody(RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setPosition(RandomGeometry.nextVector3D(random)); double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0); double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0); double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0); rootJoint.setYawPitchRoll(yaw, pitch, roll); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { double lowerLimit = oneDegreeOfFreedomJoint.getJointLowerLimit(); double upperLimit = oneDegreeOfFreedomJoint.getJointUpperLimit(); double delta = upperLimit - lowerLimit; lowerLimit = lowerLimit + 0.05 * delta; upperLimit = upperLimit - 0.05 * delta; oneDegreeOfFreedomJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); oneDegreeOfFreedomJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity)); } }
floatingJoint.setRotationAndTranslation(rotationAndTranslation); floatingJoint.setVelocity(linearVelocity); floatingJoint.setAngularVelocityInBody(sixDoFJoint.getJointTwist().getAngularPart()); originAcceleration.setIncludingFrame(spatialAccelerationVector.getLinearPart()); originAcceleration.changeFrame(ReferenceFrame.getWorldFrame()); floatingJoint.setAcceleration(originAcceleration); angularAcceleration.setIncludingFrame(spatialAccelerationVector.getAngularPart()); floatingJoint.setAngularAccelerationInBody(angularAcceleration);