RigidBodyBasics elevator = new RigidBody("elevator", worldFrame); ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); SixDoFJoint sixDoFJoint = new SixDoFJoint("sixDoF", elevator); new RigidBody("body", sixDoFJoint, getRandomDiagonalMatrix(random), mass, new Vector3D()); SpatialAcceleration rootAcceleration = new SpatialAcceleration(elevatorFrame, worldFrame, elevatorFrame); CenterOfMassAccelerationCalculator comAccelerationCalculator = new CenterOfMassAccelerationCalculator(elevator, spatialAccelerationCalculator); ReferenceFrame frameAfterJoint = sixDoFJoint.getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = sixDoFJoint.getFrameBeforeJoint(); SpatialAcceleration jointAcceleration = new SpatialAcceleration(frameAfterJoint, frameBeforeJoint, frameAfterJoint, new Vector3D(), getRandomVector(random)); sixDoFJoint.setJointPosition(getRandomVector(random)); sixDoFJoint.getJointPose().setOrientationYawPitchRoll(random.nextDouble(), random.nextDouble(), random.nextDouble()); sixDoFJoint.setJointAcceleration(jointAcceleration); elevator.updateFramesRecursively(); rotationMatrix.set(sixDoFJoint.getJointPose().getOrientation());
floatingJoint.setJointOrientation(RandomGeometry.nextQuaternion(random)); floatingJoint.setJointPosition(RandomGeometry.nextPoint3D(random, -10.0, 10.0)); Twist floatingJointTwist = MecanoRandomTools.nextTwist(random, floatingJoint.getFrameAfterJoint(), floatingJoint.getFrameBeforeJoint(), floatingJoint.getFrameAfterJoint()); floatingJoint.setJointTwist(floatingJointTwist); floatingJointInFuture.setJointConfiguration(floatingJoint); floatingJointInFuture.setJointTwist(floatingJoint); floatingJointInFuture.setJointAcceleration(floatingJoint); integrator.integrateFromVelocity(floatingJointInFuture); floatingJoint.updateFramesRecursively(); floatingJointInFuture.updateFramesRecursively();
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); }
/** * Helper method to update the robots inverse dynamics structure from the robot state. This needs to be called after * each time the state estimation updates the robot state. Some of the sensors and states use the inverse dynamics * structure internally so this makes sure the sensors stay up to date with the latest robot state estimate. */ private void updateRobot() { poseState.getTransform(imuTransform); imuJoint.setJointConfiguration(imuTransform); poseState.getTwist(imuTwist); imuJoint.setJointTwist(imuTwist); imuJoint.updateFramesRecursively(); } }
private void setRandomPosition(FloatingJoint floatingJoint, SixDoFJoint sixDoFJoint) { Point3D rootPosition = new Point3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); double yaw = random.nextDouble(); double pitch = random.nextDouble(); double roll = random.nextDouble(); floatingJoint.setPosition(rootPosition); floatingJoint.setYawPitchRoll(yaw, pitch, roll); sixDoFJoint.setJointPosition(rootPosition); sixDoFJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll); }
/** * Builds a new implementation of {@code SixDoFJointBasics}. * * @param name the joint name. * @param predecessor the predecessor of the joint. * @param transformToParent the transform to the frame after the parent joint. * @return the new 6-DoF joint. */ default SixDoFJointBasics buildSixDoFJoint(String name, RigidBodyBasics predecessor, RigidBodyTransform transformToParent) { return new SixDoFJoint(name, predecessor, transformToParent); }
SixDoFJoint sixDoFJoint = new SixDoFJoint("sixDoFJoint", elevator); new RigidBody("rigidBody", sixDoFJoint, momentOfInertia, mass, comOffset); robot.computeCOMMomentum(comPoint, linearMomentum, angularMomentum); sixDoFJoint.setJointPosition(position); sixDoFJoint.setJointOrientation(rotation); Twist jointTwist = new Twist(); jointTwist.setIncludingFrame(sixDoFJoint.getJointTwist()); jointTwist.getAngularPart().set(angularVelocity); jointTwist.getLinearPart().set(linearVelocityInBody); sixDoFJoint.setJointTwist(jointTwist); elevator.updateFramesRecursively();
/** * Gets the only leaf body, i.e. the rigid-body without any children joints, it is also called the * end-effector. * * @return the leaf body. */ public RigidBodyBasics getLeafBody() { int nRevoluteJoints = revoluteJoints.size(); if (nRevoluteJoints > 0) return revoluteJoints.get(nRevoluteJoints - 1).getSuccessor(); else return rootJoint.getSuccessor(); } }
ArrayList<RevoluteJoint> joints = new ArrayList<RevoluteJoint>(nJoints); RigidBodyBasics elevator = new RigidBody("elevator", ReferenceFrame.getWorldFrame()); SixDoFJoint sixDoFJoint = new SixDoFJoint("sixDoF", elevator); RigidBodyBasics floatingBody = new RigidBody("floatingBody", sixDoFJoint, RandomGeometry.nextDiagonalMatrix3D(random), random.nextDouble(), RandomGeometry.nextVector3D(random)); ReferenceFrame centerOfMassReferenceFrame = new CenterOfMassReferenceFrame("com", elevator.getBodyFixedFrame(), elevator); sixDoFJoint.setJointPosition(RandomGeometry.nextVector3D(random)); sixDoFJoint.getJointPose().setOrientationYawPitchRoll(random.nextDouble(), random.nextDouble(), random.nextDouble());
public TestingRobotModel() { worldFrame = ReferenceFrame.getWorldFrame(); elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); rootJoint = new SixDoFJoint("rootJoint", elevator); body = new RigidBody("body", rootJoint, Ixx, Iyy, Izz, mass, comOffset); bodyFrame = rootJoint.getFrameAfterJoint(); }
public static void main(String[] args) SixDoFJoint rootJoint = new SixDoFJoint("sixdof", elevator); RigidBodyTransform inertiaPose = new RigidBodyTransform(); Matrix3D momentOfInertia = new Matrix3D(); System.out.println(momentOfInertia); RigidBodyBasics aBody = new RigidBody("test", rootJoint, momentOfInertia, mass, inertiaPose); rootJoint.updateFramesRecursively();
SixDoFJoint footJoint = new SixDoFJoint(prefix + "FootJoint", elevator); RigidBodyBasics foot = new RigidBody(prefix + "Foot", footJoint, new Matrix3D(), 1.0, new Vector3D()); ReferenceFrame ankleFrame = foot.getBodyFixedFrame(); defaultFootPolygons.put(robotSide, footPolygon); footJoint.setJointPosition(footPositions.get(robotSide));
public static ChecksumUpdater newJointChecksumUpdater(GenericCRC32 checksum, SixDoFJoint joint) { return () -> { checksum.update(joint.getJointPose().getOrientation()); checksum.update(joint.getJointPose().getPosition()); checksum.update(joint.getJointTwist().getAngularPart()); checksum.update(joint.getJointTwist().getLinearPart()); checksum.update(joint.getJointAcceleration().getAngularPart()); checksum.update(joint.getJointAcceleration().getLinearPart()); }; }
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(); }
@Override public void doControl() { refFrame.update(); sixDofPelvisJoint.setJointConfiguration(robotPose); pelvisPoseHistoryCorrection.doControl(Conversions.secondsToNanoseconds(scs.getTime())); floatingJoint.setQuaternion(sixDofPelvisJoint.getJointPose().getOrientation()); floatingJoint.setPosition(sixDofPelvisJoint.getJointPose().getPosition()); } }
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); }
robot.addRootJoint(rootJoint); SixDoFJoint rootInverseDynamicsJoint = new SixDoFJoint("root", elevator); outputWrench.setIncludingFrame(rootInverseDynamicsJoint.getJointWrench());
public void get(double[] buffer, int offset) { QuaternionReadOnly rotation = inverseDynamicsJoint.getJointPose().getOrientation(); Tuple3DReadOnly translation = inverseDynamicsJoint.getJointPose().getPosition(); Tuple3DReadOnly angularVelocity = inverseDynamicsJoint.getJointTwist().getAngularPart(); Tuple3DReadOnly linearVelocity = inverseDynamicsJoint.getJointTwist().getLinearPart(); buffer[offset++] = rotation.getS(); buffer[offset++] = rotation.getX(); buffer[offset++] = rotation.getY(); buffer[offset++] = rotation.getZ(); buffer[offset++] = translation.getX(); buffer[offset++] = translation.getY(); buffer[offset++] = translation.getZ(); buffer[offset++] = angularVelocity.getX(); buffer[offset++] = angularVelocity.getY(); buffer[offset++] = angularVelocity.getZ(); buffer[offset++] = linearVelocity.getX(); buffer[offset++] = linearVelocity.getY(); buffer[offset] = linearVelocity.getZ(); }
/** * 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(); }
/** * Generates a 6-DoF floating joint with random physical parameters and attaches it to the given * {@code predecessor}. * * @param random the random generator to use. * @param name the joint name. * @param predecessor the rigid-body to which the joint is added as a child. * @return the random joint. */ public static SixDoFJoint nextSixDoFJoint(Random random, String name, RigidBodyBasics predecessor) { RigidBodyTransform transformToParent = predecessor.isRootBody() ? null : EuclidCoreRandomTools.nextRigidBodyTransform(random); return new SixDoFJoint(name, predecessor, transformToParent); }