private void copyAccelerationFromForwardToInverse(FloatingJoint floatingJoint, SixDoFJoint sixDoFJoint) { // Note: To get the acceleration, you can't just changeFrame on the acceleration provided by SCS. Use setBasedOnOriginAcceleration instead. // TODO: Get this to work when the FloatingJoint has an offset. Twist bodyTwist = new Twist(); bodyTwist.setIncludingFrame(sixDoFJoint.getJointTwist()); FrameVector3D originAcceleration = new FrameVector3D(sixDoFJoint.getFrameBeforeJoint()); FrameVector3D angularAcceleration = new FrameVector3D(sixDoFJoint.getFrameAfterJoint()); floatingJoint.getLinearAccelerationInWorld(originAcceleration); floatingJoint.getAngularAccelerationInBody(angularAcceleration); originAcceleration.changeFrame(sixDoFJoint.getFrameBeforeJoint()); SpatialAcceleration spatialAccelerationVector = new SpatialAcceleration(sixDoFJoint.getFrameAfterJoint(), sixDoFJoint.getFrameBeforeJoint(), sixDoFJoint.getFrameAfterJoint()); spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); sixDoFJoint.setJointAcceleration(spatialAccelerationVector); }
/** * Initialize the estimation to the provided IMU orientation. This will set the IMU velocity to zero and reset all * sensor biases. * * @param orientation of the IMU in world */ public void initialize(QuaternionReadOnly orientation) { imuTransform.setRotationAndZeroTranslation(orientation); imuTwist.setToZero(imuJoint.getFrameAfterJoint(), imuJoint.getFrameBeforeJoint(), imuJoint.getFrameAfterJoint()); poseState.initialize(imuTransform, imuTwist); linearAccelerationSensor.resetBias(); angularVelocitySensor.resetBias(); }
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); }
Twist floatingJointTwist = MecanoRandomTools.nextTwist(random, floatingJoint.getFrameAfterJoint(), floatingJoint.getFrameBeforeJoint(), floatingJoint.getFrameAfterJoint()); floatingJoint.setJointTwist(floatingJointTwist);
Twist floatingJointTwist = MecanoRandomTools.nextTwist(random, floatingJoint.getFrameAfterJoint(), floatingJoint.getFrameBeforeJoint(), floatingJoint.getFrameAfterJoint()); floatingJoint.setJointTwist(floatingJointTwist);
ReferenceFrame frameBeforeJoint = sixDoFJoint.getFrameBeforeJoint(); SpatialAcceleration jointAcceleration = new SpatialAcceleration(frameAfterJoint, frameBeforeJoint, frameAfterJoint, new Vector3D(), getRandomVector(random));