public void update(RigidBodyTransform transformBodyToWorld, FrameVector3D linearVelocity, FrameVector3D angularVelocity, FrameVector3D linearAcceleration, FrameVector3D angularAcceleration) { // Update Body Pose rootJoint.setJointConfiguration(transformBodyToWorld); // TODO correct? updateFrames(); // Update Body Velocity Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, angularVelocity, linearVelocity); rootJoint.setJointTwist(bodyTwist); // Update Body Acceleration SpatialAcceleration accelerationOfChestWithRespectToWorld = new SpatialAcceleration(bodyFrame, worldFrame, bodyFrame, angularAcceleration, linearAcceleration); accelerationOfChestWithRespectToWorld.setBaseFrame(getElevatorFrame()); rootJoint.setJointAcceleration(accelerationOfChestWithRespectToWorld); updateFrames(); }
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); }
floatingJointInFuture.setJointAcceleration(floatingJoint); integrator.integrateFromVelocity(floatingJointInFuture);
floatingJointInFuture.setJointAcceleration(floatingJoint); integrator.integrateFromVelocity(floatingJointInFuture);
sixDoFJoint.setJointPosition(getRandomVector(random)); sixDoFJoint.getJointPose().setOrientationYawPitchRoll(random.nextDouble(), random.nextDouble(), random.nextDouble()); sixDoFJoint.setJointAcceleration(jointAcceleration); elevator.updateFramesRecursively();