public void setLinearVelocity(Vector3D linearVelocityInWorld) { qrCodeJoint.setVelocity(linearVelocityInWorld); } }
public void setLinearVelocity(Vector3D linearVelocityInWorld) { qrCodeJoint.setVelocity(linearVelocityInWorld); } }
public void setVelocity(double xd, double yd, double zd) { getFloatingJoint().setVelocity(xd, yd, zd); }
public void setVelocity(Tuple3DBasics velocity) { getFloatingJoint().setVelocity(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); } }
rootJointOne.setVelocity(velocityOne); rootJointTwo.setVelocity(velocityTwo);
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.setVelocity(linearVelocityInWorld); rootJoint.setAngularVelocityInBody(angularVelocity); robot.updateVelocities();
@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()); }
pinnedJointA.setVelocity(EuclidCoreRandomTools.nextVector3D(random)); pinnedJointA.setAngularVelocityInBody(EuclidCoreRandomTools.nextVector3D(random)); pinnedJointA.setAcceleration(EuclidCoreRandomTools.nextVector3D(random));
rootJoint.setVelocity(0.0, 0.15, 0.0);
floatingJoint.setVelocity(linearVelocity); floatingJoint.setAngularVelocityInBody(sixDoFJoint.getJointTwist().getAngularPart());
Vector3D initialVelocity = new Vector3D(0.0, 0.0, -downwardVelocity); floatingJoint.setVelocity(initialVelocity); floatingJoint.setAngularVelocityInBody(new Vector3D(0.0, 0.0, 0.0)); robot.doDynamicsButDoNotIntegrate(); floatingJoint.setVelocity(initialVelocity); floatingJoint.setAngularVelocityInBody(new Vector3D(0.13, 1.0, 0.11)); robot.update();
floatingJointA.setVelocity(EuclidCoreRandomTools.nextVector3D(random)); floatingJointA.setAngularVelocityInBody(EuclidCoreRandomTools.nextVector3D(random)); floatingJointA.setAcceleration(EuclidCoreRandomTools.nextVector3D(random));
double push = 0.04; rootVelocity.setY(rootVelocity.getY() + push); rootJoint.setVelocity(rootVelocity);