public void setAngularVelocity(Vector3D angularVelocityInBody) { qrCodeJoint.setAngularVelocityInBody(angularVelocityInBody); }
public void setAngularVelocity(Vector3D angularVelocityInBody) { qrCodeJoint.setAngularVelocityInBody(angularVelocityInBody); }
public void setAngularVelocityInBody(Vector3D velocity) { getFloatingJoint().setAngularVelocityInBody(velocity); }
public static void setRandomVelocity(Random random, Robot robot) { FloatingJoint rootJoint = (FloatingJoint) robot.getRootJoints().get(0); rootJoint.setVelocity(EuclidCoreRandomTools.nextVector3D(random)); rootJoint.setAngularVelocityInBody(EuclidCoreRandomTools.nextVector3D(random)); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) joint.setQd(EuclidCoreRandomTools.nextDouble(random)); }
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); }
@Override public void update() { jointState.getRotation(rotationMatrix); jointState.getTranslation(tempVector); joint.setRotation(rotationMatrix); joint.setPosition(tempVector); jointState.getTwistAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); jointState.getTwistLinearPart(tempVector); joint.setVelocity(tempVector); } }
@Override public void update() { jointState.getRotation(rotationMatrix); jointState.getTranslation(tempVector); joint.setRotation(rotationMatrix); joint.setPosition(tempVector); jointState.getTwistAngularPart(tempVector); joint.setAngularVelocityInBody(tempVector); jointState.getTwistLinearPart(tempVector); joint.setVelocity(tempVector); } }
public void setRobotStateToMatchOtherRobot(FloatingRootJointRobot otherRobot) { otherRobot.update(); FloatingJoint floatingJoint = robot.getRootJoint(); FloatingJoint otherFloatingJoint = otherRobot.getRootJoint(); Tuple3DBasics position = new Point3D(); Tuple3DBasics velocity = new Vector3D(); otherFloatingJoint.getPositionAndVelocity(position, velocity); floatingJoint.setPositionAndVelocity(position, velocity); Quaternion rotation = new Quaternion(); otherFloatingJoint.getQuaternion(rotation); floatingJoint.setQuaternion(rotation); Vector3D angularVelocityInBody = otherFloatingJoint.getAngularVelocityInBody(); floatingJoint.setAngularVelocityInBody(angularVelocityInBody); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); ArrayList<OneDegreeOfFreedomJoint> otherOneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); otherRobot.getAllOneDegreeOfFreedomJoints(otherOneDegreeOfFreedomJoints); for (int i = 0; i < oneDegreeOfFreedomJoints.size(); i++) { OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint = oneDegreeOfFreedomJoints.get(i); OneDegreeOfFreedomJoint otherOneDegreeOfFreedomJoint = otherOneDegreeOfFreedomJoints.get(i); oneDegreeOfFreedomJoint.setQ(otherOneDegreeOfFreedomJoint.getQYoVariable().getDoubleValue()); oneDegreeOfFreedomJoint.setQd(otherOneDegreeOfFreedomJoint.getQDYoVariable().getDoubleValue()); } }
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); }
public void setRobotRootJointVelocityAndAngularVelocityToMatchFullRobotModel(FloatingJointBasics sixDoFJoint, FloatingJoint floatingJoint) { Twist rootJointTwist = new Twist(); rootJointTwist.setIncludingFrame(sixDoFJoint.getJointTwist()); floatingJoint.setAngularVelocityInBody(new Vector3D(rootJointTwist.getAngularPart())); FrameVector3D linearVelocityInWorld = new FrameVector3D(); linearVelocityInWorld.setIncludingFrame(rootJointTwist.getLinearPart()); linearVelocityInWorld.changeFrame(ReferenceFrame.getWorldFrame()); floatingJoint.setVelocity(new Vector3D(linearVelocityInWorld)); }
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)); } }
rootJoint.setRotation(rotation); rootJoint.setVelocity(linearVelocityInWorld); rootJoint.setAngularVelocityInBody(angularVelocity); robot.updateVelocities(); Point3D comPoint = new Point3D();
@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()); }
rootJoint.setAngularVelocityInBody(new Vector3d(0.1, 0.2, 0.07));
pinnedJointA.setAngularVelocityInBody(EuclidCoreRandomTools.nextVector3D(random)); pinnedJointA.setAcceleration(EuclidCoreRandomTools.nextVector3D(random)); pinnedJointA.setAngularAccelerationInBody(EuclidCoreRandomTools.nextVector3D(random));
rootJoint.setAngularVelocityInBody(rotationAxis);
pin2.setQd(-pin1.getQDYoVariable().getDoubleValue()); Vector3D angularVelocityInBody = new Vector3D(0.0, pin1.getQDYoVariable().getDoubleValue(), 0.0); root2.setAngularVelocityInBody(angularVelocityInBody);
linearVelocity.changeFrame(ReferenceFrame.getWorldFrame()); floatingJoint.setVelocity(linearVelocity); floatingJoint.setAngularVelocityInBody(sixDoFJoint.getJointTwist().getAngularPart());
floatingJoint.setAngularVelocityInBody(new Vector3D(0.0, 0.0, 0.0)); robot.doDynamicsButDoNotIntegrate(); floatingJoint.setAngularVelocityInBody(new Vector3D(0.13, 1.0, 0.11)); robot.update(); robot.updateVelocities();
floatingJointA.setAngularVelocityInBody(EuclidCoreRandomTools.nextVector3D(random)); floatingJointA.setAcceleration(EuclidCoreRandomTools.nextVector3D(random)); floatingJointA.setAngularAccelerationInBody(EuclidCoreRandomTools.nextVector3D(random));