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; }
@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); } };
public void getPose(Point3d pointToPack, Quat4d quaternionToPack) { getPosition(pointToPack); getOrientation(quaternionToPack); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { currentDistortionPose.changeFrame(parentFrame); currentDistortionPose.getPose(transformToParent); } };
public void setPose(FramePoint position, FrameOrientation orientation) { setPosition(position); setOrientation(orientation); }
leftFootPose.setToZero(referenceFrames.getSoleFrame(RobotSide.LEFT)); rightFootPose.setToZero(referenceFrames.getSoleFrame(RobotSide.RIGHT)); leftFootPose.changeFrame(ReferenceFrame.getWorldFrame()); rightFootPose.changeFrame(ReferenceFrame.getWorldFrame()); leftFootPose.getPosition(temp); pointBetweenFeet.set(temp); rightFootPose.getPosition(temp); pointBetweenFeet.add(temp); pointBetweenFeet.scale(0.5); goalPose.getPosition(goalPosition); vectorFromFeetToGoal.sub(goalPosition, pointBetweenFeet); goalPose.setPosition(shorterGoalPosition); goalPose.setOrientation(goalOrientation); tempStanceFootPose.set(leftFootPose); goalPose.setZ(leftFootPose.getZ()); tempStanceFootPose.set(rightFootPose); goalPose.setZ(rightFootPose.getZ()); xyGoal.setX(goalPose.getX()); xyGoal.setY(goalPose.getY()); double distanceFromXYGoal = 1.0; footstepPlannerGoal.setXYGoal(xyGoal, distanceFromXYGoal);
public void setGoalPose(FramePose goalPose) { goalPose.getPosition2dIncludingFrame(this.goalPosition); Tuple3d goalPosition = new Point3d(); goalPose.getPosition(goalPosition); FootstepPlannerGoal footstepPlannerGoal = new FootstepPlannerGoal(); footstepPlannerGoal.setGoalPoseBetweenFeet(goalPose); Point2d xyGoal = new Point2d(); xyGoal.setX(goalPose.getX()); xyGoal.setY(goalPose.getY()); double distanceFromXYGoal = 1.0; footstepPlannerGoal.setXYGoal(xyGoal, distanceFromXYGoal); footstepPlannerGoal.setFootstepPlannerGoalType(FootstepPlannerGoalType.CLOSE_TO_XY_POSITION); FramePose leftFootPose = new FramePose(); leftFootPose.setToZero(referenceFrames.getSoleFrame(RobotSide.LEFT)); leftFootPose.changeFrame(ReferenceFrame.getWorldFrame()); sendPacketToUI(new UIPositionCheckerPacket(new Point3d(xyGoal.getX(), xyGoal.getY(), leftFootPose.getZ()), new Quat4d())); footstepPlanner.setGoal(footstepPlannerGoal); footstepPlanner.setInitialStanceFoot(leftFootPose, RobotSide.LEFT); }
private void updateTangentialCircleFrame() { if (controlledFrame != null) { tangentialSteeringFramePose.setToZero(controlledFrame); } else { tangentialSteeringFramePose.setToZero(currentPosition.getReferenceFrame()); tangentialSteeringFramePose.setPosition(currentPosition); } tangentialSteeringFramePose.changeFrame(steeringWheelFrame); double x = tangentialSteeringFramePose.getX(); double y = tangentialSteeringFramePose.getY(); double yaw = trimAngleMinusPiToPi(Math.PI / 2.0 + Math.atan2(y, x)); tangentialSteeringFramePose.setYawPitchRoll(yaw, 0.0, 0.0); tangentialSteeringFrame.setPoseAndUpdate(tangentialSteeringFramePose); yoTangentialSteeringFramePose.setAndMatchFrame(tangentialSteeringFramePose); }
private ContactableSelectableBoxRobot createDebrisRobot(FramePose debrisPose) { debrisPose.checkReferenceFrameMatch(constructionWorldFrame); ContactableSelectableBoxRobot debris = ContactableSelectableBoxRobot.createContactable2By4Robot(debrisName + String.valueOf(id++), debrisDepth, debrisWidth, debrisLength, debrisMass); debris.setPosition(debrisPose.getX(), debrisPose.getY(), debrisPose.getZ()); debris.setYawPitchRoll(debrisPose.getYaw(), debrisPose.getPitch(), debrisPose.getRoll()); return debris; }
private void setGoalAndInitialFootClosestToGoal(FramePose goalPose) tempLeftFootPose.setToZero(referenceFrames.getFootFrame(RobotSide.LEFT)); tempRightFootPose.setToZero(referenceFrames.getFootFrame(RobotSide.RIGHT)); tempLeftFootPose.changeFrame(ReferenceFrame.getWorldFrame()); tempRightFootPose.changeFrame(ReferenceFrame.getWorldFrame()); javax.vecmath.Vector3d vectorFromFeetToGoal = new javax.vecmath.Vector3d(); tempLeftFootPose.getPosition(temp); pointBetweenFeet.set(temp); tempLeftFootPose.getPosition(temp); pointBetweenFeet.add(temp); pointBetweenFeet.scale(0.5); goalPose.getPosition(vectorFromFeetToGoal); vectorFromFeetToGoal.sub(pointBetweenFeet); goalPose.setOrientation(goalOrientation); tempStanceFootPose.set(tempLeftFootPose); goalPose.setZ(tempLeftFootPose.getZ()); tempStanceFootPose.set(tempRightFootPose); goalPose.setZ(tempRightFootPose.getZ());
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
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; }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { midFeetPose.setToZero(midFeetZUpWalkDirectionFrame); pelvisPose.setToZero(getPelvisFrame()); pelvisPose.changeFrame(midFeetZUpWalkDirectionFrame); midFeetPose.setX(pelvisPose.getX()); midFeetPose.changeFrame(ReferenceFrame.getWorldFrame()); midFeetPose.getPose(transformToParent); } };
private void calculateInitialFootPoseAndOffsets() { SideDependentList<Footstep> currentFootsteps; if (priorStanceFeetSpecified) currentFootsteps = priorStanceFeet; else currentFootsteps = createFootstepsFromBipedFeet(); Footstep left = currentFootsteps.get(RobotSide.LEFT); Footstep right = currentFootsteps.get(RobotSide.RIGHT); FramePose leftPose = new FramePose(); FramePose rightPose = new FramePose(); left.getSolePose(leftPose); right.getSolePose(rightPose); FramePose2d leftPose2d = new FramePose2d(); FramePose2d rightPose2d = new FramePose2d(); leftPose.getPose2dIncludingFrame(leftPose2d); rightPose.getPose2dIncludingFrame(rightPose2d); startPose.interpolate(leftPose2d, rightPose2d, 0.5); Pose2dReferenceFrame startFramePose = new Pose2dReferenceFrame("StartPoseFrame", startPose); leftPose.changeFrame(startFramePose); rightPose.changeFrame(startFramePose); FrameOrientation2d leftOrientation = new FrameOrientation2d(); FrameOrientation2d rightOrientation = new FrameOrientation2d(); leftPose.getOrientation2dIncludingFrame(leftOrientation); rightPose.getOrientation2dIncludingFrame(rightOrientation); initialDeltaFeetYaw = leftOrientation.sub(rightOrientation); initialDeltaFeetY = leftPose.getY() - rightPose.getY(); initialDeltaFeetX = leftPose.getX() - rightPose.getX(); }
@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); }
public void setPose(FramePose pose) { pose.getOrientation(tempYawPitchRoll); setYawPitchRoll(tempYawPitchRoll); x.set(pose.getX()); y.set(pose.getY()); z.set(pose.getZ()); }
public void computeFootTrajectoryMessage(RobotSide robotSide) { checkIfDataHasBeenSet(); Point3d desiredPosition = new Point3d(); Quat4d desiredOrientation = new Quat4d(); ReferenceFrame footFrame = fullRobotModelToUseForConversion.getEndEffectorFrame(robotSide, LimbName.LEG); FramePose desiredFootPose = new FramePose(footFrame); desiredFootPose.changeFrame(worldFrame); desiredFootPose.getPose(desiredPosition, desiredOrientation); FootTrajectoryMessage footTrajectoryMessage = new FootTrajectoryMessage(robotSide, trajectoryTime, desiredPosition, desiredOrientation); output.setFootTrajectoryMessage(footTrajectoryMessage); }
private void setXYVectorFromPoseToPoseNormalize(YoFrameVector2d vectorToPack, FramePose fromPose, FramePose toPose) { if (fromPose.epsilonEquals(toPose, 1e-7, Double.MAX_VALUE)) { vectorToPack.set(fromPose.getReferenceFrame(), 0.0, 0.0); } else { FrameVector2d frameTuple2d = vectorToPack.getFrameTuple2d(); frameTuple2d.setByProjectionOntoXYPlane(toPose.getFramePointCopy()); fromPose.checkReferenceFrameMatch(vectorToPack); frameTuple2d.sub(fromPose.getX(), fromPose.getY()); frameTuple2d.normalize(); vectorToPack.setWithoutChecks(frameTuple2d); } }
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 updateTransformToParent(RigidBodyTransform transformToParent) { originPose.checkReferenceFrameMatch(parentFrame); originPose.getPose(transformToParent); }