public void updateJointPositions_ID_to_SCS() { if (scsFloatingJoint != null) { idFloatingJoint.getFrameAfterJoint().getTransformToDesiredFrame(transformToWorld, ReferenceFrame.getWorldFrame()); scsFloatingJoint.setRotationAndTranslation(transformToWorld); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); scsJoint.setQ(idJoint.getQ()); } }
private void getFloatingJointStateFromSCS() { FloatingJoint scsJoint = (FloatingJoint) robot.getRootJoints().get(0); RigidBodyTransform jointTransform3D = scsJoint.getJointTransform3D(); floatingJoint.getJointPose().set(jointTransform3D); floatingJoint.getFrameAfterJoint().update(); FrameVector3D linearVelocity = new FrameVector3D(); scsJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(floatingJoint.getFrameAfterJoint()); floatingJoint.getJointTwist().set(scsJoint.getAngularVelocityInBody(), linearVelocity); }
public void setFullRobotModelRootJointPositionAndOrientationToMatchRobot(FloatingJointBasics sixDoFJoint, FloatingJoint floatingJoint) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); floatingJoint.getTransformToWorld(transformToWorld); sixDoFJoint.setJointConfiguration(transformToWorld); }
/** * Integrates the given {@code joint}'s acceleration and velocity to update its velocity and * configuration. * * @param joint the joint to integrate the state of. The joint configuration is modified. */ public void doubleIntegrateFromAcceleration(FloatingJointBasics joint) { doubleIntegrate(joint.getJointAcceleration(), joint.getJointTwist(), joint.getJointPose()); }
/** * Integrates the given {@code joint}'s velocity to update its configuration. * * @param joint the joint to integrate the state of. The joint configuration is modified. */ public void integrateFromVelocity(FloatingJointBasics joint) { integrate(joint.getJointTwist(), joint.getJointPose()); }
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); }
position.set(rootJoint.getJointPose().getPosition()); linearVelocity.set(rootJoint.getJointTwist().getLinearPart()); newAngularVelocity.add(angularVelocity); rootJoint.setJointOrientation(newOrientation); rootJoint.setJointPosition(newPosition); rootJoint.updateFramesRecursively(); frameLinearVelocity.setIncludingFrame(ReferenceFrame.getWorldFrame(), newLinearVelocity); frameAngularVelocity.setIncludingFrame(ReferenceFrame.getWorldFrame(), newAngularVelocity); frameLinearVelocity.scale(velocityDecay); frameAngularVelocity.scale(velocityDecay); frameLinearVelocity.changeFrame(rootJoint.getFrameAfterJoint()); frameAngularVelocity.changeFrame(rootJoint.getFrameAfterJoint()); rootJointTwist.setIncludingFrame(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), frameAngularVelocity, frameLinearVelocity); rootJoint.setJointTwist(rootJointTwist);
public void setFullRobotModelStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity) { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = rootJoint.getFrameAfterJoint(); Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity), RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setJointTwist(bodyTwist); rootJoint.setJointPosition(RandomGeometry.nextVector3D(random)); double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0); double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0); double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0); rootJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll); ArrayList<OneDoFJointBasics> oneDoFJoints = new ArrayList<OneDoFJointBasics>(); fullRobotModel.getOneDoFJoints(oneDoFJoints); for (OneDoFJointBasics oneDoFJoint : oneDoFJoints) { double lowerLimit = oneDoFJoint.getJointLimitLower(); double upperLimit = oneDoFJoint.getJointLimitUpper(); double delta = upperLimit - lowerLimit; lowerLimit = lowerLimit + 0.05 * delta; upperLimit = upperLimit - 0.05 * delta; oneDoFJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); oneDoFJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity)); } }
sixDoFJoint.setJointConfiguration(positionAndRotation); ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint(); sixDoFJoint.setJointTwist(bodyTwist); sixDoFJoint.updateFramesRecursively();
public void setSixDoFJointAccelerationRandomly(FloatingJointBasics sixDoFJoint, Random random, double maxRootJointLinearAcceleration, double maxRootJointAngularAcceleration) { // 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, RandomGeometry.nextVector3D(random, maxRootJointLinearAcceleration)); FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame, RandomGeometry.nextVector3D(random, maxRootJointAngularAcceleration)); // originAcceleration.changeFrame(elevatorFrame); SpatialAcceleration spatialAccelerationVector = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); sixDoFJoint.setJointAcceleration(spatialAccelerationVector); }
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); }
/** {@inheritDoc} */ @Override default void setJointPosition(Tuple3DReadOnly jointTranslation) { getJointPose().setPosition(jointTranslation); }
if (updatePositions) sixDoFJoint.getJointConfiguration(rotationAndTranslation); floatingJoint.setRotationAndTranslation(rotationAndTranslation); ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint(); linearVelocity.setIncludingFrame(pelvisFrame, sixDoFJoint.getJointTwist().getLinearPart()); linearVelocity.changeFrame(ReferenceFrame.getWorldFrame()); floatingJoint.setVelocity(linearVelocity); floatingJoint.setAngularVelocityInBody(sixDoFJoint.getJointTwist().getAngularPart()); spatialAccelerationVector.setIncludingFrame(sixDoFJoint.getJointAcceleration()); originAcceleration.setIncludingFrame(spatialAccelerationVector.getLinearPart()); originAcceleration.changeFrame(ReferenceFrame.getWorldFrame());
private void compareJointAccelerations(double epsilon) { boolean areEqual = true; String errorMessage = "Joint accelerations are not equal\n"; String jointTriggerErrorMessage = ""; FloatingJoint scsFloatingJoint = (FloatingJoint) robot.getRootJoints().get(0); SpatialAcceleration expected = extractFromFloatingJoint(scsFloatingJoint, floatingJoint.getFrameBeforeJoint(), floatingJoint.getFrameAfterJoint()); if (!floatingJoint.getJointAcceleration().epsilonEquals(expected, epsilon)) { areEqual = false; jointTriggerErrorMessage = " floating-joint"; } errorMessage += "Floating joint:\nExpected: " + expected + "\nActual : " + floatingJoint.getJointAcceleration(); for (OneDoFJointBasics joint : allOneDoFJoints) { String jointName = joint.getName(); OneDegreeOfFreedomJoint scsJoint = (OneDegreeOfFreedomJoint) robot.getJoint(jointName); if (!EuclidCoreTools.epsilonEquals(scsJoint.getQDD(), joint.getQdd(), epsilon)) { areEqual = false; jointTriggerErrorMessage += " " + jointName; } errorMessage += "\n" + jointName + ",\texpected: " + String.format(FORMAT, scsJoint.getQDD()) + ",\tactual: " + String.format(FORMAT, joint.getQdd()) + ", difference: " + String.format(FORMAT, Math.abs(scsJoint.getQDD() - joint.getQdd())); } if (!areEqual) { throw new RuntimeException(errorMessage + "\nJoint(s) triggering error:" + jointTriggerErrorMessage); } }
public void copyAccelerationFromForwardToInverseBroken(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint) { ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint(); FrameVector3D angularAccelerationInBody = new FrameVector3D(); floatingJoint.getAngularAcceleration(angularAccelerationInBody, bodyFrame); FrameVector3D linearAccelerationInBody = new FrameVector3D(); floatingJoint.getLinearAcceleration(linearAccelerationInBody); linearAccelerationInBody.changeFrame(bodyFrame); SpatialAcceleration jointAcceleration = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); jointAcceleration.getLinearPart().set(linearAccelerationInBody); jointAcceleration.getAngularPart().set(angularAccelerationInBody); sixDoFJoint.setJointAcceleration(jointAcceleration); }
ReferenceFrame rootJointFrame = randomizedFullRobotModel.getRootJoint().getFrameAfterJoint(); ReferenceFrame supportFootFrame = randomizedFullRobotModel.getFoot(supportFootSide).getBodyFixedFrame(); RigidBodyTransform transformFromRootJointToWorldFrame = rootJointFrame.getTransformToDesiredFrame(supportFootFrame); transformFromRootJointToWorldFrame.preMultiply(initialSupportFootTransform); randomizedFullRobotModel.getRootJoint().setJointConfiguration(transformFromRootJointToWorldFrame); randomizedFullRobotModel.updateFrames();
public void updateJointVelocities_ID_to_SCS() { if (scsFloatingJoint != null) { ReferenceFrame rootBodyFrame = idFloatingJoint.getFrameAfterJoint(); rootJointTwist.setIncludingFrame(idFloatingJoint.getJointTwist()); linearVelocity.setIncludingFrame(rootJointTwist.getLinearPart()); angularVelocity.setIncludingFrame(rootJointTwist.getAngularPart()); linearVelocity.changeFrame(ReferenceFrame.getWorldFrame()); angularVelocity.changeFrame(rootBodyFrame); scsFloatingJoint.setVelocity(linearVelocity); scsFloatingJoint.setAngularVelocityInBody(angularVelocity); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); scsJoint.setQd(idJoint.getQd()); } }
rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = robotConfigurationData.getRootOrientation(); rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS()); rootJoint.getPredecessor().updateFramesRecursively();
rootJoint.setJointConfiguration(0, outputForRootJoint.getDesiredConfiguration()); rootJoint.setJointVelocity(0, outputForRootJoint.getDesiredVelocity()); rootJoint.getPredecessor().updateFramesRecursively(); else oneDoFJoints[0].getPredecessor().updateFramesRecursively();
desiredRootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ()); Quaternion orientation = robotConfigurationData.getRootOrientation(); desiredRootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS()); desiredRootJoint.setJointVelocity(0, new DenseMatrix64F(6, 1)); desiredRootJoint.getPredecessor().updateFramesRecursively();