public Vector3d getActualAngularAccelerationInBodyFrame() { return rootJoint.getAngularAccelerationInBody(); }
private double findAccelerationGreatestMagnitude() { FloatingJoint scsFloatingJoint = (FloatingJoint) robot.getRootJoints().get(0); double mag = scsFloatingJoint.getAngularAccelerationInBody().length(); Vector3D linearAcceleration = new Vector3D(); scsFloatingJoint.getLinearAccelerationInWorld(linearAcceleration); mag = Math.max(linearAcceleration.length(), mag); ArrayList<OneDegreeOfFreedomJoint> joints = new ArrayList<>(); robot.getAllOneDegreeOfFreedomJoints(joints); for (OneDegreeOfFreedomJoint joint : joints) mag = Math.max(mag, Math.abs(joint.getQDD())); return mag; } }
public void copyAccelerationFromForwardToInverse(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint) { // Note: To get the acceleration, you can't just changeFrame on the acceleration provided by SCS. Use setBasedOnOriginAcceleration instead. ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint(); Twist bodyTwist = new Twist(); bodyTwist.setIncludingFrame(sixDoFJoint.getJointTwist()); FrameVector3D originAcceleration = new FrameVector3D(elevatorFrame); FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame); floatingJoint.getLinearAccelerationInWorld(originAcceleration); floatingJoint.getAngularAccelerationInBody(angularAcceleration); originAcceleration.changeFrame(elevatorFrame); SpatialAcceleration spatialAccelerationVector = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); sixDoFJoint.setJointAcceleration(spatialAccelerationVector); }
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; } }
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); }
temp1.set(root1.getAngularAccelerationInBody());
public boolean checkFullRobotModelRootJointAccelerationmatchesRobot(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint, double epsilon) { // Note: To get the acceleration, you can't just changeFrame on the acceleration provided by SCS. Use setBasedOnOriginAcceleration instead. ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint(); Twist bodyTwist = new Twist(); bodyTwist.setIncludingFrame(sixDoFJoint.getJointTwist()); FrameVector3D originAcceleration = new FrameVector3D(elevatorFrame); FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame); floatingJoint.getLinearAccelerationInWorld(originAcceleration); floatingJoint.getAngularAccelerationInBody(angularAcceleration); originAcceleration.changeFrame(elevatorFrame); //TODO: These should be from the inverse dynamics to the SCS frames... originAcceleration.changeFrame(ReferenceFrame.getWorldFrame()); computedRootJointLinearAcceleration.set(originAcceleration); computedRootJointAngularAcceleration.set(angularAcceleration); SpatialAcceleration spatialAccelerationVectorOfSimulatedRootJoint = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); spatialAccelerationVectorOfSimulatedRootJoint.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); SpatialAcceleration spatialAccelerationVectorOfInverseDynamicsRootJoint = new SpatialAcceleration(); spatialAccelerationVectorOfInverseDynamicsRootJoint.setIncludingFrame(sixDoFJoint.getJointAcceleration()); return spatialAccelerationVectorOfInverseDynamicsRootJoint.epsilonEquals(spatialAccelerationVectorOfInverseDynamicsRootJoint, epsilon); }
floatingJoint.getAngularAccelerationInBody(angularAcceleration.getVector()); originAcceleration.changeFrame(sixDoFJoint.getFrameBeforeJoint());