public void getPosition(Point3d pointToPack) { originPose.getPosition(pointToPack); }
public void getLidarPosition(Point3d positionToPack) { lidarPose.getPosition(positionToPack); }
public void getPose(Vector3d vector3dToPack, Quat4d quaternionToPack) { getPosition(vector3dToPack); getOrientation(quaternionToPack); }
public void getPose(Point3d pointToPack, Quat4d quaternionToPack) { getPosition(pointToPack); getOrientation(quaternionToPack); }
public void getPose(Vector3d vector3dToPack, AxisAngle4d axisAngleToPack) { getPosition(vector3dToPack); getOrientation(axisAngleToPack); }
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 rotatePoseAboutAxis(ReferenceFrame rotationAxisFrame, Axis rotationAxis, double angle, boolean lockPosition, boolean lockOrientation) { ReferenceFrame initialFrame = this.referenceFrame; this.changeFrame(rotationAxisFrame); RigidBodyTransform axisRotationTransform = new RigidBodyTransform(); TransformTools.rotate(axisRotationTransform, angle, rotationAxis); if (!lockPosition) { Vector3d tempVector = new Vector3d(); this.getPosition(tempVector); axisRotationTransform.transform(tempVector); this.setPosition(tempVector); } if(!lockOrientation) this.pose.getOrientation().applyTransform(axisRotationTransform); this.changeFrame(initialFrame); }
@Override public void doControl() { if (fiducialDetectorBehaviorService.getGoalHasBeenLocated()) { fiducialDetectorBehaviorService.getReportedGoalPoseWorldFrame(foundFiducialPose); foundFiducialYoFramePose.set(foundFiducialPose); foundFiducial.set(true); Point3d position = new Point3d(); foundFiducialPose.getPosition(position); sendTextToSpeechPacket("Target object located at " + position); } else { sendTextToSpeechPacket("Target object not located"); } if (headTrajectorySentTimer.totalElapsed() < 2.0) { return; } pitchHeadToFindFiducial(); // pitchHeadToCenterFiducial(); }
public static FramePose setupStanceFoot(SideDependentList<PoseReferenceFrame> soleFrames, SideDependentList<SixDoFJoint> sixDoFJoints) { FramePose startStanceFootPose = new FramePose(worldFrame, new Point3d(0.0, 0.2, 0.0), new Quat4d()); soleFrames.get(RobotSide.LEFT).setPoseAndUpdate(startStanceFootPose); soleFrames.get(RobotSide.LEFT).update(); Point3d stanceAnklePosition = new Point3d(); startStanceFootPose.getPosition(stanceAnklePosition); sixDoFJoints.get(RobotSide.LEFT).setPosition(stanceAnklePosition); return startStanceFootPose; }
@Override protected void setBehaviorInput() { FramePose goalPose = new FramePose(); lookForGoalBehavior.getGoalPose(goalPose); Tuple3d goalPosition = new Point3d(); goalPose.getPosition(goalPosition); String xString = FormattingTools.getFormattedToSignificantFigures(goalPosition.getX(), 3); String yString = FormattingTools.getFormattedToSignificantFigures(goalPosition.getY(), 3); TextToSpeechPacket p1 = new TextToSpeechPacket("Plannning Footsteps to (" + xString + ", " + yString + ")"); sendPacket(p1); planHumanoidFootstepsBehavior.setGoalPoseAndFirstSwingSide(goalPose, nextSideToSwing.getEnumValue()); } };
public static FramePose setupSwingFoot(SideDependentList<PoseReferenceFrame> soleFrames, SideDependentList<SixDoFJoint> sixDoFJoints) { FramePose startSwingFootPose = new FramePose(worldFrame, new Point3d(0.0, -0.2, 0.0), new Quat4d()); soleFrames.get(RobotSide.RIGHT).setPoseAndUpdate(startSwingFootPose); soleFrames.get(RobotSide.RIGHT).update(); Point3d swingAnklePosition = new Point3d(); startSwingFootPose.getPosition(swingAnklePosition); sixDoFJoints.get(RobotSide.RIGHT).setPosition(swingAnklePosition); return startSwingFootPose; }
@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)); }
@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)); }
@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; }
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; }