public void getVelocity(Tuple3DBasics velocityToPack) { getFloatingJoint().getVelocity(velocityToPack); }
public void setFullRobotModelRootJointVelocityAndAngularVelocityToMatchRobot(FloatingJointBasics sixDoFJoint, FloatingJoint floatingJoint) { FrameVector3D angularVelocityFrameVector = new FrameVector3D(); FrameVector3D linearVelocityFrameVector = new FrameVector3D(); 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); }
private void getFloatingJointStateFromSCS() { FloatingJoint scsJoint = (FloatingJoint) robot.getRootJoints().get(0); RigidBodyTransform jointTransform3D = scsJoint.getJointTransform3D(); floatingJoint.getJointPose().set(jointTransform3D); floatingJoint.getFrameAfterJoint().update(); FrameVector3D linearVelocity = new FrameVector3D(); scsJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(floatingJoint.getFrameAfterJoint()); floatingJoint.getJointTwist().set(scsJoint.getAngularVelocityInBody(), linearVelocity); }
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 static SpatialAcceleration extractFromFloatingJoint(FloatingJoint floatingJoint, ReferenceFrame frameBeforeJoint, ReferenceFrame frameAfterJoint) { ReferenceFrame elevatorFrame = frameBeforeJoint; ReferenceFrame bodyFrame = frameAfterJoint; FrameVector3D angularVelocityFrameVector = new FrameVector3D(); FrameVector3D linearVelocityFrameVector = new FrameVector3D(); floatingJoint.getVelocity(linearVelocityFrameVector); linearVelocityFrameVector.changeFrame(bodyFrame); floatingJoint.getAngularVelocity(angularVelocityFrameVector, bodyFrame); Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, angularVelocityFrameVector, linearVelocityFrameVector); FrameVector3D originAcceleration = new FrameVector3D(elevatorFrame); FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame); floatingJoint.getLinearAccelerationInWorld(originAcceleration); floatingJoint.getAngularAccelerationInBody(angularAcceleration); originAcceleration.changeFrame(elevatorFrame); SpatialAcceleration spatialAcceleration = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); spatialAcceleration.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); return spatialAcceleration; } }
ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint(); floatingJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(pelvisFrame);
ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint(); floatingJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(pelvisFrame);
rootJoint.getVelocity(rootVelocity); double push = 0.04; rootVelocity.setY(rootVelocity.getY() + push);
EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(0.0, 0.0, mass * deltaVelocity), impulseInWorldToPack, 1e-7); Vector3D velocity = new Vector3D(); floatingJoint.getVelocity(velocity);