public void getOrientation(Quat4d quaternionToPack) { originPose.getOrientation(quaternionToPack); }
public void getOrientation(Matrix3d matrixToPack) { originPose.getOrientation(matrixToPack); }
public void getLidarOrientation(Quat4d orientationToPack) { lidarPose.getOrientation(orientationToPack); }
public void getPose(Point3d pointToPack, Quat4d quaternionToPack) { getPosition(pointToPack); getOrientation(quaternionToPack); }
public void getPose(Vector3d vector3dToPack, AxisAngle4d axisAngleToPack) { getPosition(vector3dToPack); getOrientation(axisAngleToPack); }
public void getPose(Vector3d vector3dToPack, Quat4d quaternionToPack) { getPosition(vector3dToPack); getOrientation(quaternionToPack); }
public static void packFramePoseInJMEQuaternion(FramePose original, Quaternion target) { Quat4d quat4d = new Quat4d(); original.getOrientation(quat4d); packVectMathQuat4dInJMEQuaternion(quat4d, target); }
public void set(FramePose initialStanceFootPose, RobotSide initialStanceSide, FramePose goalPose) { this.initialStanceSide = initialStanceSide; Point3d tempPoint = new Point3d(); Quat4d tempOrientation = new Quat4d(); initialStanceFootPose.getPosition(tempPoint); initialStanceFootPose.getOrientation(tempOrientation); stanceFootPositionInWorld = new Point3f(tempPoint); stanceFootOrientationInWorld = new Quat4f(tempOrientation); goalPose.getPosition(tempPoint); goalPose.getOrientation(tempOrientation); goalPositionInWorld = new Point3f(tempPoint); goalOrientationInWorld = new Quat4f(tempOrientation); }
public void setPose(FramePose framePose) { yoFramePoint.checkReferenceFrameMatch(framePose.getReferenceFrame()); framePose.getPosition(tempPoint); yoFramePoint.setPoint(tempPoint); framePose.getOrientation(tempQuaternion); yoFrameOrientation.set(tempQuaternion); }
public void setPose(FramePose pose) { pose.getOrientation(tempYawPitchRoll); setYawPitchRoll(tempYawPitchRoll); x.set(pose.getX()); y.set(pose.getY()); z.set(pose.getZ()); }
private void sendPelvisOrientationTrajectoryCommand() { userDesiredPelvisPose.getFramePoseIncludingFrame(framePose); framePose.changeFrame(worldFrame); double time = userDesiredPelvisPoseTrajectoryTime.getDoubleValue(); framePose.getOrientation(orientation); orientationCommand.clear(); orientationCommand.addTrajectoryPoint(time, orientation, zeroVelocity); orientationCommand.setExecutionMode(ExecutionMode.OVERRIDE); orientationCommand.setCommandId(Packet.VALID_MESSAGE_DEFAULT_ID); if (DEBUG) System.out.println("Submitting " + orientationCommand); controllerCommandInputManager.submitCommand(orientationCommand); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { for (RobotSide robotSide : RobotSide.values) { ReferenceFrame footFrame = getFootFrame(robotSide); footFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame()); FramePose footPose = footPoses.get(robotSide); footPose.setPoseIncludingFrame(ReferenceFrame.getWorldFrame(), tempTransform); footPose.getOrientation(tempYawPitchRoll); tempYawPitchRoll[1] = 0.0; tempYawPitchRoll[2] = 0.0; footPose.setYawPitchRoll(tempYawPitchRoll); } midFootZUpPose.interpolate(leftFootPose, rightFootPose, 0.5); midFootZUpPose.setZ(Math.min(leftFootPose.getZ(), rightFootPose.getZ())); midFootZUpPose.getPose(transformToParent); } };
private FootstepDataListMessage createFootstepDataListFromPlan(FootstepPlan plan, int maxNumberOfStepsToTake) { FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); footstepDataListMessage.setDefaultSwingTime(0.5); footstepDataListMessage.setDefaultTransferTime(0.1); int lastStepIndex = Math.min(maxNumberOfStepsToTake + 1, plan.getNumberOfSteps()); for (int i = 1; i < lastStepIndex; i++) { SimpleFootstep footstep = plan.getFootstep(i); footstep.getSoleFramePose(tempFirstFootstepPose); tempFirstFootstepPose.getPosition(tempFootstepPosePosition); tempFirstFootstepPose.getOrientation(tempFirstFootstepPoseOrientation); // sendTextToSpeechPacket("Sending footstep " + footstep.getRobotSide() + " " + tempFootstepPosePosition + " " + tempFirstFootstepPoseOrientation); FootstepDataMessage firstFootstepMessage = new FootstepDataMessage(footstep.getRobotSide(), new Point3d(tempFootstepPosePosition), new Quat4d(tempFirstFootstepPoseOrientation)); firstFootstepMessage.setOrigin(FootstepOrigin.AT_SOLE_FRAME); footstepDataListMessage.add(firstFootstepMessage); } footstepDataListMessage.setExecutionMode(ExecutionMode.OVERRIDE); return footstepDataListMessage; }
@Override public void onBehaviorEntered() { hasTargetBeenProvided.set(false); haveFootstepsBeenGenerated.set(false); hasInputBeenSet.set(false); footstepListBehavior.initialize(); robotPose.setToZero(fullRobotModel.getRootJoint().getFrameAfterJoint()); robotPose.changeFrame(worldFrame); robotYoPose.set(robotPose); robotPose.getPosition(robotLocation); robotPose.getOrientation(robotOrientation); this.targetLocation.set(robotLocation); this.targetOrientation.set(robotOrientation); }
private FootstepDataListMessage createFootstepDataListFromPlan(FootstepPlan plan, int maxNumberOfStepsToTake) { FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); footstepDataListMessage.setDefaultSwingTime(0.5); footstepDataListMessage.setDefaultTransferTime(0.1); int lastStepIndex = Math.min(maxNumberOfStepsToTake + 1, plan.getNumberOfSteps()); for (int i = 1; i < lastStepIndex; i++) { SimpleFootstep footstep = plan.getFootstep(i); footstep.getSoleFramePose(tempFirstFootstepPose); tempFirstFootstepPose.getPosition(tempFootstepPosePosition); tempFirstFootstepPose.getOrientation(tempFirstFootstepPoseOrientation); // sendTextToSpeechPacket("Sending footstep " + footstep.getRobotSide() + " " + tempFootstepPosePosition + " " + tempFirstFootstepPoseOrientation); FootstepDataMessage firstFootstepMessage = new FootstepDataMessage(footstep.getRobotSide(), new Point3d(tempFootstepPosePosition), new Quat4d(tempFirstFootstepPoseOrientation)); firstFootstepMessage.setOrigin(FootstepOrigin.AT_SOLE_FRAME); footstepDataListMessage.add(firstFootstepMessage); } footstepDataListMessage.setExecutionMode(ExecutionMode.OVERRIDE); return footstepDataListMessage; }
private FootstepDataListMessage createFootstepDataListFromPlan(FootstepPlan plan, int maxNumberOfStepsToTake, double swingTime, double transferTime) { FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); footstepDataListMessage.setDefaultSwingTime(swingTime); footstepDataListMessage.setDefaultTransferTime(transferTime); int lastStepIndex = Math.min(maxNumberOfStepsToTake + 1, plan.getNumberOfSteps()); for (int i = 1; i < lastStepIndex; i++) { SimpleFootstep footstep = plan.getFootstep(i); footstep.getSoleFramePose(tempFirstFootstepPose); tempFirstFootstepPose.getPosition(tempFootstepPosePosition); tempFirstFootstepPose.getOrientation(tempFirstFootstepPoseOrientation); FootstepDataMessage firstFootstepMessage = new FootstepDataMessage(footstep.getRobotSide(), new Point3d(tempFootstepPosePosition), new Quat4d(tempFirstFootstepPoseOrientation)); firstFootstepMessage.setOrigin(FootstepOrigin.AT_SOLE_FRAME); footstepDataListMessage.add(firstFootstepMessage); } footstepDataListMessage.setExecutionMode(ExecutionMode.OVERRIDE); return footstepDataListMessage; }
private FootstepDataListMessage createFootstepDataListFromPlan(FootstepPlan plan, int maxNumberOfStepsToTake) { FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); footstepDataListMessage.setDefaultSwingTime(swingTime.getDoubleValue()); footstepDataListMessage.setDefaultTransferTime(transferTime.getDoubleValue()); int lastStepIndex = Math.min(maxNumberOfStepsToTake + 1, plan.getNumberOfSteps()); for (int i = 1; i < lastStepIndex; i++) { SimpleFootstep footstep = plan.getFootstep(i); footstep.getSoleFramePose(tempFirstFootstepPose); tempFirstFootstepPose.getPosition(tempFootstepPosePosition); tempFirstFootstepPose.getOrientation(tempFirstFootstepPoseOrientation); // sendTextToSpeechPacket("Sending footstep " + footstep.getRobotSide() + " " + tempFootstepPosePosition + " " + tempFirstFootstepPoseOrientation); FootstepDataMessage firstFootstepMessage = new FootstepDataMessage(footstep.getRobotSide(), new javax.vecmath.Point3d(tempFootstepPosePosition), new Quat4d(tempFirstFootstepPoseOrientation)); footstepDataListMessage.add(firstFootstepMessage); } footstepDataListMessage.setExecutionMode(ExecutionMode.OVERRIDE); return footstepDataListMessage; }
private void sequenceStepsInPlace() { FootstepDataListMessage footstepDataList = new FootstepDataListMessage(swingTime.getDoubleValue(), transferTime.getDoubleValue()); FramePose footstepPose = new FramePose(); for (int i = 0; i < numberOfCyclesToRun.getIntegerValue(); i++) { for (RobotSide robotSide : RobotSide.values()) { footstepPose.setToZero(fullRobotModel.getEndEffectorFrame(robotSide, LimbName.LEG)); footstepPose.changeFrame(worldFrame); Point3d footLocation = new Point3d(); Quat4d footOrientation = new Quat4d(); footstepPose.getPosition(footLocation); footstepPose.getOrientation(footOrientation); FootstepDataMessage footstepData = new FootstepDataMessage(robotSide, footLocation, footOrientation); footstepDataList.add(footstepData); } } pipeLine.submitSingleTaskStage(new FootstepListTask(footstepListBehavior, footstepDataList)); }
public Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFramePose.setIncludingFrame(desiredPose); errorFramePose.changeFrame(controlFrame); errorFramePose.getPosition(errorPosition); errorFramePose.getOrientation(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(errorAxisAngle.getAngle()); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }
public Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude) { errorFramePose.setIncludingFrame(desiredPose); errorFramePose.changeFrame(controlFrame); errorFramePose.getPosition(errorPosition); errorFramePose.getOrientation(errorAxisAngle); errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle())); ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame(); Twist desiredTwist = new Twist(); desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation); desiredTwist.getMatrix(spatialError, 0); subspaceError.reshape(selectionMatrix.getNumRows(), 1); CommonOps.mult(selectionMatrix, spatialError, subspaceError); errorMagnitude.setValue(NormOps.normP2(subspaceError)); desiredTwist.scale(1.0 / updateDT); return desiredTwist; }