private void computeRootJointTwistLinearPart(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointLinearVelocity) { rootJoint.getJointTwist(tempRootJointTwistExisting); tempRootJointTwistExisting.checkReferenceFramesMatch(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint()); tempRootJointTwistExisting.getAngularPart(tempRootJointTwistExistingAngularPart); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), tempRootJointTwistExistingAngularPart.getVector()); }
private void computeRootJointAcceleration(FloatingInverseDynamicsJoint rootJoint, SpatialAccelerationVector rootJointAcceleration, FrameVector rootJointAngularAcceleration, FrameVector rootJointLinearAcceleration) { rootJointAngularAcceleration.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointLinearAcceleration.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointAcceleration.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearAcceleration.getVector(), rootJointAngularAcceleration.getVector()); }
private void readAndUpdateRootJointAngularAndLinearVelocity() { ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = rootJoint.getFrameAfterJoint(); FrameVector linearVelocity = robot.getRootJointVelocity(); linearVelocity.changeFrame(pelvisFrame); FrameVector angularVelocity = robot.getRootJointAngularVelocityInRootJointFrame(pelvisFrame); angularVelocity.changeFrame(pelvisFrame); Twist bodyTwist = new Twist(pelvisFrame, elevatorFrame, pelvisFrame, linearVelocity.getVector(), angularVelocity.getVector()); rootJoint.setJointTwist(bodyTwist); }
private void computeRootJointTwist(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointAngularVelocity, FrameVector rootJointLinearVelocity) { rootJointAngularVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), rootJointAngularVelocity.getVector()); }
ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint(); originAcceleration.setToZero(sixDoFJoint.getFrameBeforeJoint()); angularAcceleration.setToZero(sixDoFJoint.getFrameAfterJoint()); originAcceleration.changeFrame(sixDoFJoint.getFrameBeforeJoint()); spatialAccelerationVector.setToZero(sixDoFJoint.getFrameAfterJoint(), sixDoFJoint.getFrameBeforeJoint(), sixDoFJoint.getFrameAfterJoint()); spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist);