public void setPositionAndRotation(RigidBodyTransform transform) { RotationTools.convertTransformToQuaternion(transform, jointRotation); if (!RotationTools.isQuaternionNormalized(jointRotation)) { throw new AssertionError("quaternion is not normalized. " + jointRotation); } transform.getTranslation(jointTranslation); this.afterJointFrame.setRotation(jointRotation); this.afterJointFrame.setTranslation(jointTranslation); }
public void setRotation(Quat4d jointRotation) { this.jointRotation.set(jointRotation); this.afterJointFrame.setRotation(jointRotation); }
public void recordCurrentState() { FloatingInverseDynamicsJoint rootJoint = fullRobotModel.getRootJoint(); FloatingInverseDynamicsJointReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint(); rootJointFrame.getTraslation(rootJointTranslation); rootJointFrame.getRotation(rootJointRotation); yoRootJointTranslation.set(rootJointTranslation); yoRootJointRotation.set(rootJointRotation); }
public void setPosition(double x, double y, double z) { jointTranslation.set(x, y, z); this.afterJointFrame.setTranslation(this.jointTranslation); }
public void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); updateRootJointRotation(inverseDynamicsStructure.getRootJoint(), orientationPort.getData(), estimationFrame); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); updateRootJointTwistAngularPart(twistCalculator, rootJoint, angularVelocityPort.getData()); rootJoint.getFrameAfterJoint().update(); twistCalculator.compute(); }
private void computeRootJointToWorldTransform(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, RigidBodyTransform rootJointToWorldToPack, RigidBodyTransform estimationLinkTransform) { // H_{root}^{estimation} rootJoint.getFrameAfterJoint().getTransformToDesiredFrame(tempRootJointFrameToEstimationFrame, estimationFrame); // H_{root}^{w} = H_{estimation}^{w} * H_{root}^{estimation} rootJointToWorldToPack.set(estimationLinkTransform); rootJointToWorldToPack.multiply(tempRootJointFrameToEstimationFrame); }
public PlanarJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame) { super(name, predecessor, beforeJointFrame); this.afterJointFrame = new FloatingInverseDynamicsJointReferenceFrame(name, beforeJointFrame); this.jointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAccelerationDesired = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); }
public void setRotation(Matrix3d jointRotation) { RotationTools.convertMatrixToQuaternion(jointRotation, this.jointRotation); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // this.jointRotation.set(jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }
public void setPosition(Tuple3d jointTranslation) { this.jointTranslation.set(jointTranslation); this.afterJointFrame.setTranslation(this.jointTranslation); }
public void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); centerOfMassCalculator.compute(); updateRootJointPosition(rootJoint, estimationFrame, centerOfMassPositionOutputPort.getData()); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); updateRootJointTwistLinearPart(centerOfMassVelocityOutputPort.getData(), rootJoint); twistCalculator.compute(); }
private void computeRootJointTransform(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, RigidBodyTransform rootJointToWorldToPack, RigidBodyTransform estimationLinkTransform) { // H_{root}^{estimation} rootJoint.getFrameAfterJoint().getTransformToDesiredFrame(tempRootJointFrameToEstimationFrame, estimationFrame); // H_{root}^{w} = H_{estimation}^{w} * H_{root}^{estimation} rootJointToWorldToPack.set(estimationLinkTransform); rootJointToWorldToPack.multiply(tempRootJointFrameToEstimationFrame); } }
public SphericalJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame) { super(name, predecessor, beforeJointFrame); this.afterJointFrame = new FloatingInverseDynamicsJointReferenceFrame(name, beforeJointFrame); this.jointAngularVelocity = new FrameVector(afterJointFrame); this.jointAngularAcceleration = new FrameVector(afterJointFrame); this.jointAngularAccelerationDesired = new FrameVector(afterJointFrame); }
public void setPositionAndRotation(RigidBodyTransform transform) { RotationTools.convertTransformToYawPitchRoll(transform, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, jointRotation); if (!RotationTools.isQuaternionNormalized(jointRotation)) { throw new AssertionError("quaternion is not normalized. " + jointRotation); } transform.getTranslation(jointTranslation); jointTranslation.setY(0.0); this.afterJointFrame.setRotation(jointRotation); this.afterJointFrame.setTranslation(jointTranslation); }
public void setRotation(double yaw, double pitch, double roll) { RotationTools.convertYawPitchRollToQuaternion(0.0, pitch, 0.0, jointRotation); afterJointFrame.setRotation(this.jointRotation); }
public void setPosition(double x, double y, double z) { jointTranslation.set(x, 0.0, z); afterJointFrame.setTranslation(this.jointTranslation); }
public void run() { FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); ReferenceFrame estimationFrame = inverseDynamicsStructure.getEstimationFrame(); centerOfMassCalculator.compute(); updateRootJointConfiguration(rootJoint, estimationFrame); rootJoint.getFrameAfterJoint().update(); TwistCalculator twistCalculator = inverseDynamicsStructure.getTwistCalculator(); SpatialAccelerationCalculator spatialAccelerationCalculator = inverseDynamicsStructure.getSpatialAccelerationCalculator(); updateRootJointTwistAndSpatialAcceleration(twistCalculator, spatialAccelerationCalculator); twistCalculator.compute(); spatialAccelerationCalculator.compute(); }
private void computeRootJointToWorldTransform(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, RigidBodyTransform rootJointToWorldToPack, RigidBodyTransform estimationLinkTransform) { // H_{root}^{estimation} rootJoint.getFrameAfterJoint().getTransformToDesiredFrame(tempRootJointFrameToEstimationFrame, estimationFrame); // H_{root}^{w} = H_{estimation}^{w} * H_{root}^{estimation} rootJointToWorldToPack.set(estimationLinkTransform); rootJointToWorldToPack.multiply(tempRootJointFrameToEstimationFrame); }
public SixDoFJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame) { super(name, predecessor, beforeJointFrame); this.afterJointFrame = new FloatingInverseDynamicsJointReferenceFrame(name, beforeJointFrame); this.jointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAcceleration = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); this.jointAccelerationDesired = new SpatialAccelerationVector(afterJointFrame, beforeJointFrame, afterJointFrame); }
@Override public void setConfiguration(DenseMatrix64F matrix, int rowStart) { int index = rowStart; double qRot = matrix.get(index++, 0); double x = matrix.get(index++, 0); double z = matrix.get(index++, 0); RotationTools.convertYawPitchRollToQuaternion(0.0, qRot, 0.0, jointRotation); jointTranslation.set(x, 0.0, z); afterJointFrame.setRotation(jointRotation); afterJointFrame.setTranslation(jointTranslation); }
public void setRotation(Matrix3d jointRotation) { RotationTools.convertMatrixToQuaternion(jointRotation, this.jointRotation); // DON'T USE THIS: the method in Quat4d is flawed and doesn't work for some rotation matrices! // this.jointRotation.set(jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }