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 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; } }
linearVelocity.changeFrame(pelvisFrame); floatingJoint.getAngularVelocity(angularVelocity, pelvisFrame);
linearVelocity.changeFrame(pelvisFrame); floatingJoint.getAngularVelocity(angularVelocity, pelvisFrame);