public FramePose(ReferenceFrame referenceFrame, RigidBodyTransform transform) { this(referenceFrame); setPose(transform); }
public void setPoseIncludingFrame(ReferenceFrame referenceFrame, Point3d position, Quat4d orientation) { setPose(position, orientation); this.referenceFrame = referenceFrame; }
public void setPoseIncludingFrame(ReferenceFrame referenceFrame, Point3f position, Quat4f orientation) { setPose(position, orientation); this.referenceFrame = referenceFrame; }
public FramePose(ReferenceFrame referenceFrame, Tuple3d point3d, AxisAngle4d axisAngle4d) { this(referenceFrame); setPose(point3d, axisAngle4d); }
public FramePose(ReferenceFrame referenceFrame, Tuple3d position, Quat4d orientation) { this(referenceFrame, new Pose()); setPose(position, orientation); }
public void setPoseAndUpdate(FramePose pose) { originPose.setPose(pose); this.update(); }
public void setPoseAndUpdate(Point3d position, Quat4d orientation) { originPose.setPose(position, orientation); this.update(); }
public void setPoseAndUpdate(PoseReferenceFrame poseReferenceFrame) { originPose.setPose(poseReferenceFrame.originPose); this.update(); }
public void setPoseAndUpdate(FramePoint position, FrameOrientation orientation) { originPose.setPose(position, orientation); this.update(); }
public void setPoseAndUpdate(RigidBodyTransform transformToParent) { originPose.setPose(transformToParent); this.update(); }
public Footstep createFootstep(RobotSide robotSide, Point3d position, Quat4d orientation) { FramePose footstepPose = new FramePose(); footstepPose.setPose(position, orientation); return createFootstep(robotSide, footstepPose); }
public void setPoseIncludingFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(orientation); referenceFrame = position.referenceFrame; setPose(position.getPoint(), orientation.getQuaternion()); }
private void changeControlFrame(ReferenceFrame oldControlFrame, ReferenceFrame newControlFrame, FramePose framePoseToModify) { if (oldControlFrame == newControlFrame) return; framePoseToModify.getPose(oldTrackingFrameDesiredTransform); newControlFrame.getTransformToDesiredFrame(transformFromNewTrackingFrameToOldTrackingFrame, oldControlFrame); newTrackingFrameDesiredTransform.multiply(oldTrackingFrameDesiredTransform, transformFromNewTrackingFrameToOldTrackingFrame); framePoseToModify.setPose(newTrackingFrameDesiredTransform); }
private void changeDesiredPoseBodyFrame(ReferenceFrame oldBodyFrame, ReferenceFrame newBodyFrame, FramePose framePoseToModify) { if (oldBodyFrame == newBodyFrame) return; framePoseToModify.getPose(oldBodyFrameDesiredTransform); newBodyFrame.getTransformToDesiredFrame(transformFromNewBodyFrameToOldBodyFrame, oldBodyFrame); newBodyFrameDesiredTransform.multiply(oldBodyFrameDesiredTransform, transformFromNewBodyFrameToOldBodyFrame); framePoseToModify.setPose(newBodyFrameDesiredTransform); }
private void createValve(String valveRobotName, ValveType valveType, double x, double y, double z, double yaw_degrees, double forceVectorScale) { FramePose valvePose = new FramePose(ReferenceFrame.getWorldFrame()); Point3d position = new Point3d(x, y, z); Quat4d orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(Math.toRadians(yaw_degrees), Math.toRadians(0), Math.toRadians(0), orientation); valvePose.setPose(position, orientation); ContactableValveRobot valve = new ContactableValveRobot(valveRobotName, valveType, 0.5, valvePose); valve.createValveRobot(); valve.createAvailableContactPoints(1, 30, forceVectorScale, true); valveRobot.add(valve); }
private FramePose generateDebrisPose(Point3d positionWithRespectToRobot, double debrisYaw, double debrisPitch, double debrisRoll) { FramePose debrisPose = new FramePose(robotInitialPoseReferenceFrame); Quat4d orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(debrisYaw, debrisPitch, debrisRoll, orientation); debrisPose.setPose(positionWithRespectToRobot, orientation); debrisPose.changeFrame(constructionWorldFrame); return debrisPose; }
private void createRotatedZUpCylinder(String cylinderName, double height, double radius, double x, double y, double z, double yaw_degrees, double pitch_degrees, double roll_degrees, double forceVectorScale) { FramePose cylinderPose = new FramePose(ReferenceFrame.getWorldFrame()); Point3d position = new Point3d(x, y, z); Quat4d orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(Math.toRadians(yaw_degrees), Math.toRadians(pitch_degrees), Math.toRadians(0), orientation); cylinderPose.setPose(position, orientation); RigidBodyTransform cylinderTransformToWorld = new RigidBodyTransform(); cylinderPose.getPose(cylinderTransformToWorld); ContactableStaticCylinderRobot cylinder = new ContactableStaticCylinderRobot(cylinderName, cylinderTransformToWorld, height, radius, YoAppearance.Red()); cylinder.createAvailableContactPoints(1, 30, forceVectorScale, true); cylinderRobots.add(cylinder); }
@Override public void doControl() { wristJoint.getTransformToWorld(transform); wristJointPose.setPose(transform); yoWristJointPose.set(wristJointPose); wristJoint.getTransformToWorld(transform); transform.transform(wristToHandControlFrame, tangentVector); handControlFramePose.setPose(transform); handControlFramePose.translate(tangentVector); yoHandControlFramePose.set(handControlFramePose); handControlFramePose.getPosition(handControlFramePositionInWorld); efpHandControlFrame.setPosition(handControlFramePositionInWorld); // efpGravity.setForce(0.0, 0.0, -gGRAVITY * MASSDRILL); // efpGravity.setForce(0.0, 0.0, 0.0); // efpWrist.setForce(exponentialCutForceModel(efpHandControlFrame)); efpWrist.setForce(quadraticCutForceModel(efpHandControlFrame)); }
private void updateSensorPosition() { sensorFrame.update(); sensorPose.setPose(sensorFrame.getTransformToDesiredFrame(world)); sensorPose.getPositionIncludingFrame(temp); yoSensorPositionInWorld.set(temp.getReferenceFrame(), temp.getX(), temp.getY(), temp.getZ()); }
public void doControl(long timestamp) { if (pelvisPoseCorrectionCommunicator != null) { pelvisReferenceFrame.update(); checkForNewPacket(); pelvisReferenceFrame.getTransformToDesiredFrame(stateEstimatorPelvisTransformInWorld, worldFrame); outdatedPoseUpdater.putStateEstimatorTransformInBuffer(stateEstimatorPelvisTransformInWorld, timestamp); offsetErrorInterpolator.interpolateError(correctedPelvisPoseInWorldFrame); /////for SCS feedback stateEstimatorInWorldFramePose.setPose(stateEstimatorPelvisTransformInWorld); yoStateEstimatorInWorldFramePose.set(stateEstimatorInWorldFramePose); ///// updateCorrectedPelvis(); pelvisReferenceFrame.update(); checkForNeedToSendCorrectionUpdate(); } }