@Override public void getBodyTransformToWorld(RigidBodyTransform transformToWorld) { transformToWorld.set(originalSteeringWheelPose); }
public void translate(double x, double y, double z) { tempTransform.setTranslationAndIdentityRotation(x, y, z); transform.set(transform); transform.multiply(tempTransform); }
public void rotateEuler(Vector3DReadOnly eulerAngles) { tempTransform.setRotationEulerAndZeroTranslation(eulerAngles); transform.set(transform); transform.multiply(tempTransform); }
public void rotate(RotationMatrixReadOnly rotationMatrix) { tempTransform.setRotationAndZeroTranslation(rotationMatrix); transform.set(transform); transform.multiply(tempTransform); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newPose = new RigidBodyTransform(); pinJointTransform.setRotationYawAndZeroTranslation(steeringWheelPinJoint.getQYoVariable().getDoubleValue()); newPose.set(originalSteeringWheelPose); newPose.multiply(pinJointTransform); steeringWheelFrame.setPoseAndUpdate(newPose); super.updateAllGroundContactPointVelocities(); }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.setAndInvert(transformShoulderToEndEffector); transformEndEffectorToDesired.set(transformEndEffectorToShoulder); transformEndEffectorToDesired.multiply(transformShoulderToDesired); }
@Override public void getBodyTransformToWorld(RigidBodyTransform transformToWorld) { transformToWorld.set(pinJointTransformToWorld); } }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform sliderJointTransform = new RigidBodyTransform(); RigidBodyTransform newButtonPose = new RigidBodyTransform(); buttonPushVector.scale(buttonSliderJoint.getQYoVariable().getDoubleValue()); sliderJointTransform.setTranslationAndIdentityRotation(buttonPushVector); buttonPushVector.normalize(); newButtonPose.set(originalButtonTransform); newButtonPose.multiply(sliderJointTransform); buttonFrame.setPoseAndUpdate(newButtonPose); super.updateAllGroundContactPointVelocities(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(mocapHeadPoseInZUp); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0); } };
/** * Sets the pose, i.e. position and orientation, of this shape. * * @param rigidBodyTransform rigid-body transform holding the new position and orientation for this * shape. Not modified. */ public final void setPose(RigidBodyTransform rigidBodyTransform) { shapePose.set(rigidBodyTransform); }
/** * Sets the pose, i.e. position and orientation, of this shape to the pose of {@code other}. * * @param other shape holding the pose with the new position and orientation for this shape. Not * modified. */ public void setPose(Shape3D<S> other) { shapePose.set(other.shapePose); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(rigidBodyInformationInMocapOrigin); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(originalTransform); }
public void setTransformAndUpdate(RigidBodyTransform transform) { this.transform3D.set(transform); this.update(); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(originalFrame.getTransformToRoot()); }
public void setRequestedBehaviorControlMode(BehaviorControlModeEnum newRequestedControlMode) { if (!this.requestedControlMode.equals(newRequestedControlMode)) { this.requestedControlMode = newRequestedControlMode; behaviorStatus.put(newRequestedControlMode, behavior.getBehaviorStatus()); testFrameTransformToWorld.get(newRequestedControlMode).set(getCurrentTestFrameTransformToWorld()); referenceFramesForLogging.put(newRequestedControlMode, robotDataReceiver.getUpdatedReferenceFramesCopy()); } }
/** {@inheritDoc} */ @Override default void getJointConfiguration(RigidBodyTransform jointTransform) { jointTransform.set(getJointPose().getOrientation(), getJointPose().getPosition()); } }
public SO3TrajectoryControllerCommand(Random random) { int randomNumberOfPoints = random.nextInt(16) + 1; for (int i = 0; i < randomNumberOfPoints; i++) { trajectoryPointList.addTrajectoryPoint(RandomNumbers.nextDoubleWithEdgeCases(random, 0.01), RandomGeometry.nextQuaternion(random), RandomGeometry.nextVector3D(random)); } trajectoryFrame = EuclidFrameRandomTools.nextReferenceFrame("trajectoryFrame", random, ReferenceFrame.getWorldFrame()); controlFramePoseInBodyFrame.set(RandomGeometry.nextQuaternion(random), RandomGeometry.nextVector3D(random)); useCustomControlFrame = random.nextBoolean(); }
@Override public void set(LoadBearingCommand other) { load = other.getLoad(); coefficientOfFriction = other.getCoefficientOfFriction(); bodyFrameToContactFrame.set(other.getBodyFrameToContactFrame()); contactNormalInWorldFrame.set(other.getContactNormalInWorldFrame()); }