us.ihmc.robotics.robotSide.RobotSide.negateIfLeftSide()方法的使用及代码示例

x33g5p2x  于2022-01-29 转载在 其他  
字(15.0k)|赞(0)|评价(0)|浏览(96)

本文整理了Java中us.ihmc.robotics.robotSide.RobotSide.negateIfLeftSide方法的一些代码示例，展示了RobotSide.negateIfLeftSide的具体用法。这些代码示例主要来源于Github/Stackoverflow/Maven等平台，是从一些精选项目中提取出来的代码，具有较强的参考意义，能在一定程度帮忙到你。RobotSide.negateIfLeftSide方法的具体详情如下：
包路径：us.ihmc.robotics.robotSide.RobotSide
类名称：RobotSide
方法名：negateIfLeftSide

RobotSide.negateIfLeftSide介绍

暂无

代码示例

代码示例来源：origin: us.ihmc/ihmc-whole-body-controller

private double[] symmetricArmPose(double d0, double d1, double d2, double d3, RobotSide robotSide)
{
 return new double[] {d0, robotSide.negateIfLeftSide(d1), d2, robotSide.negateIfLeftSide(d3)};
}

代码示例来源：origin: us.ihmc/IHMCWholeBodyController

private double[] symmetricArmPose(double d0, double d1, double d2, double d3, RobotSide robotSide)
{
 return new double[] {d0, robotSide.negateIfLeftSide(d1), d2, robotSide.negateIfLeftSide(d3)};
}

代码示例来源：origin: us.ihmc/thor

private void createTransforms()
{
 for (RobotSide robotSide : RobotSide.values())
 {
   Vector3f centerOfHandToWristTranslation = new Vector3f();
   float[] angles = new float[3];
   centerOfHandToWristTranslation = new Vector3f(0f, robotSide.negateIfLeftSide(0.015f), -0.06f);
   angles[0] = (float) robotSide.negateIfLeftSide(Math.toRadians(90));
   angles[1] = 0.0f;
   angles[2] = (float) robotSide.negateIfLeftSide(Math.toRadians(90));
   //
   Quaternion centerOfHandToWristRotation = new Quaternion(angles);
   offsetHandFromWrist.set(robotSide, new Transform(centerOfHandToWristTranslation, centerOfHandToWristRotation));
 }
}

代码示例来源：origin: us.ihmc/valkyrie

private void createTransforms()
{
 for (RobotSide robotSide : RobotSide.values())
 {
   Vector3f centerOfHandToWristTranslation = new Vector3f();
   float[] angles = new float[3];
   centerOfHandToWristTranslation = new Vector3f(0f, robotSide.negateIfLeftSide(0.015f), -0.06f);
   angles[0] = (float) robotSide.negateIfLeftSide(Math.toRadians(90));
   angles[1] = 0.0f;
   angles[2] = (float) robotSide.negateIfLeftSide(Math.toRadians(90));
   //
   Quaternion centerOfHandToWristRotation = new Quaternion(angles);
   offsetHandFromWrist.set(robotSide, new Transform(centerOfHandToWristTranslation, centerOfHandToWristRotation));
 }
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setSymmetricExitCMPConstantOffsets(double exitCMPForwardOffset, double exitCMPInsideOffset)
{
 for (RobotSide robotSide : RobotSide.values)
 {
   YoFrameVector2d exitCMPUserOffset = exitCMPUserOffsets.get(robotSide);
   exitCMPUserOffset.setX(exitCMPForwardOffset);
   exitCMPUserOffset.setY(robotSide.negateIfLeftSide(exitCMPInsideOffset));
 }
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void setSymmetricEntryCMPConstantOffsets(double entryCMPForwardOffset, double entryCMPInsideOffset)
{
 for (RobotSide robotSide : RobotSide.values)
 {
   YoFrameVector2d entryCMPUserOffset = entryCMPUserOffsets.get(robotSide);
   entryCMPUserOffset.setX(entryCMPForwardOffset);
   entryCMPUserOffset.setY(robotSide.negateIfLeftSide(entryCMPInsideOffset));
 }
}

代码示例来源：origin: us.ihmc/DarpaRoboticsChallenge

private double getYawForFoot(FootstepDataMessage stanceFoot, FramePose2d currentPose, FramePose2d nextPose, double maxYaw)
  {
   double stanceYaw = RotationTools.computeYaw(stanceFoot.getOrientation());
   double prospectiveYaw = currentPose.getYaw();
   double dYaw = AngleTools.trimAngleMinusPiToPi(prospectiveYaw - stanceYaw);
   double sign = stanceFoot.getRobotSide().negateIfLeftSide(1.0);

   if (sign * dYaw < 0.0)
   {
     prospectiveYaw = stanceYaw;
   }

   if (sign * dYaw > maxYaw)
   {
     prospectiveYaw = stanceYaw + sign * maxYaw;
   }

   return prospectiveYaw;
  }
}

代码示例来源：origin: us.ihmc/ihmc-footstep-planning-test

private Point3D computeBodyPoint(FramePose3D solePose, RobotSide side, FootstepPlannerParameters parameters, double bodyHeight)
{
 double stanceWidth = parameters.getIdealFootstepWidth();
 ReferenceFrame soleFrame = new PoseReferenceFrame("stanceFrame", solePose);
 FramePoint3D bodyPosition = new FramePoint3D(soleFrame);
 bodyPosition.setY(side.negateIfLeftSide(stanceWidth / 2.0));
 bodyPosition.changeFrame(ReferenceFrame.getWorldFrame());
 Point3D bodyPoint = new Point3D(bodyPosition);
 bodyPoint.addZ(bodyHeight);
 return bodyPoint;
}

代码示例来源：origin: us.ihmc/simulation-construction-set-tools

private void createHandleGraphics()
{
 // graphics - handle hinge
 RotationMatrix rotX90 = new RotationMatrix();
 rotX90.setToRollMatrix(Math.PI / 2.0);
 doorHandleGraphics.rotate(rotX90);
 doorHandleGraphics.translate(new Vector3D(0.0, 0.0, -0.5 * depthY)); // center graphics
 
 for(RobotSide robotSide : RobotSide.values())
 {
   doorHandleGraphics.addCylinder(robotSide.negateIfLeftSide(handleDoorSeparation+0.5*depthY), handleHingeRadius, YoAppearance.Grey());
   // graphics - handle
   double translation = robotSide.negateIfLeftSide(handleDoorSeparation + 0.5*depthY);
   doorHandleGraphics.translate(new Vector3D(0.0, 0.0, translation));
      doorHandleGraphics.rotate(new AxisAngle(0.0, 1.0, 0.0, robotSide.negateIfRightSide(0.5 * Math.PI)));
   doorHandleGraphics.addCylinder(robotSide.negateIfLeftSide(handleLength), handleRadius, YoAppearance.Grey());
   doorHandleGraphics.rotate(new AxisAngle(0.0, 1.0, 0.0, -robotSide.negateIfRightSide(0.5 * Math.PI)));
      doorHandleGraphics.translate(new Vector3D(0.0, 0.0, -translation)); 
 }
 
 handleLink.setLinkGraphics(doorHandleGraphics);
}

代码示例来源：origin: us.ihmc/ihmc-humanoid-robotics

public static FramePoint3D getCenterOfFootstep(Footstep stance, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 FramePoint3D stanceCenter = new FramePoint3D(stance.getSoleReferenceFrame());
 stanceCenter.getReferenceFrame().checkReferenceFrameMatch(stance.getSoleReferenceFrame());
 stanceCenter.setX(stanceCenter.getX() + centimetersForwardFromCenter);
 stanceCenter.setY(stanceCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 stanceCenter.changeFrame(worldFrame);
 return stanceCenter;
}

代码示例来源：origin: us.ihmc/IHMCHumanoidRobotics

public static FramePoint getCenterOfFootstep(Footstep stance, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 FramePoint stanceCenter = new FramePoint(stance.getSoleReferenceFrame());
 stanceCenter.getReferenceFrame().checkReferenceFrameMatch(stance.getSoleReferenceFrame());
 stanceCenter.setX(stanceCenter.getX() + centimetersForwardFromCenter);
 stanceCenter.setY(stanceCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 stanceCenter.changeFrame(worldFrame);
 return stanceCenter;
}

代码示例来源：origin: us.ihmc/CommonWalkingControlModules

public void updateReachabilityConstraintForSingleSupport(RobotSide supportSide, ICPOptimizationSolver solver)
{
 solver.setNumberOfReachabilityVertices(4);
 double lateralInnerLimit = lateralReachabilityInnerLimit.getDoubleValue();
 double lateralOuterLimit = lateralReachabilityOuterLimit.getDoubleValue();
 lateralInnerLimit = supportSide.negateIfLeftSide(lateralInnerLimit);
 lateralOuterLimit = supportSide.negateIfLeftSide(lateralOuterLimit);
 ReferenceFrame supportSoleFrame = bipedSupportPolygons.getSoleZUpFrames().get(supportSide);
 tempVertex.setToZero(supportSoleFrame);
 tempVertex.set(forwardReachabilityLimit.getDoubleValue(), lateralInnerLimit);
 solver.setReachabilityVertex(0, tempVertex, supportSoleFrame);
 tempVertex.setToZero(supportSoleFrame);
 tempVertex.set(forwardReachabilityLimit.getDoubleValue(), lateralOuterLimit);
 solver.setReachabilityVertex(1, tempVertex, supportSoleFrame);
 tempVertex.setToZero(supportSoleFrame);
 tempVertex.set(backwardReachabilityLimit.getDoubleValue(), lateralInnerLimit);
 solver.setReachabilityVertex(2, tempVertex, supportSoleFrame);
 tempVertex.setToZero(supportSoleFrame);
 tempVertex.set(backwardReachabilityLimit.getDoubleValue(), lateralOuterLimit);
 solver.setReachabilityVertex(3, tempVertex, supportSoleFrame);
}

代码示例来源：origin: us.ihmc/valkyrie

private void useDefaultAngles()
{
 setSetpoint(jointMap.getSpineJointName(SpineJointName.SPINE_YAW), 0.0);
 setSetpoint(jointMap.getSpineJointName(SpineJointName.SPINE_PITCH), -0.06);
 setSetpoint(jointMap.getSpineJointName(SpineJointName.SPINE_ROLL), 0.0);
 for (RobotSide robotSide : RobotSide.values)
 {
   setSetpoint(jointMap.getLegJointName(robotSide, LegJointName.HIP_YAW), 0.0);
   setSetpoint(jointMap.getLegJointName(robotSide, LegJointName.HIP_ROLL), 0.0);
   setSetpoint(jointMap.getLegJointName(robotSide, LegJointName.HIP_PITCH), -0.25);
   setSetpoint(jointMap.getLegJointName(robotSide, LegJointName.KNEE_PITCH), 0.7);
   setSetpoint(jointMap.getLegJointName(robotSide, LegJointName.ANKLE_PITCH), 0.0);
   setSetpoint(jointMap.getLegJointName(robotSide, LegJointName.ANKLE_ROLL), 0.0);
   setSetpoint(jointMap.getArmJointName(robotSide, ArmJointName.SHOULDER_PITCH), 0.3);
   setSetpoint(jointMap.getArmJointName(robotSide, ArmJointName.SHOULDER_ROLL), robotSide.negateIfLeftSide(1.1708));
   setSetpoint(jointMap.getArmJointName(robotSide, ArmJointName.SHOULDER_YAW), 0.05);
   setSetpoint(jointMap.getArmJointName(robotSide, ArmJointName.ELBOW_PITCH), robotSide.negateIfLeftSide(1.7));
 }
}

代码示例来源：origin: us.ihmc/IHMCHumanoidRobotics

public static FramePoint getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 Point2d footstepCenter;
 
 List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints();
 if (predictedContactPoints != null)
 {
   ConvexPolygon2d contactPolygon = new ConvexPolygon2d(predictedContactPoints);
   footstepCenter = contactPolygon.getCentroid();
 }
 else
 {
   footstepCenter = new Point2d();
 }
 
 footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter);
 footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 
 FramePoint footstepCenter3d = new FramePoint(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0);
 footstepCenter3d.changeFrame(worldFrame);
 return footstepCenter3d;
}

代码示例来源：origin: us.ihmc/ihmc-footstep-planning

public void setInitialStanceFoot(FramePose3D stanceFootPose, RobotSide side)
{
 if (side == null)
 {
   if (debug)
    PrintTools.info("Start node needs a side, but trying to set it to null. Setting it to " + defaultStartNodeSide);
   side = defaultStartNodeSide;
 }
 double defaultStepWidth = parameters.getIdealFootstepWidth();
 ReferenceFrame stanceFrame = new PoseReferenceFrame("stanceFrame", stanceFootPose);
 FramePoint2D bodyStartPoint = new FramePoint2D(stanceFrame);
 bodyStartPoint.setY(side.negateIfLeftSide(defaultStepWidth / 2.0));
 bodyStartPoint.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame());
 bodyStartPose.setToZero(ReferenceFrame.getWorldFrame());
 bodyStartPose.setPosition(bodyStartPoint.getX(), bodyStartPoint.getY(), 0.0);
 bodyStartPose.setOrientationYawPitchRoll(stanceFootPose.getYaw(), 0.0, 0.0);
}

代码示例来源：origin: us.ihmc/ihmc-quadruped-robotics-test

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testAreLegsCrossing()
{
 QuadrupedSupportPolygon simple = createSimplePolygon();
 assertFalse("cross", QuadrupedSupportPolygonTools.areLegsCrossing(simple));
 for (RobotQuadrant robotQuadrant : RobotQuadrant.values)
 {
   double negateIfLeftSide = robotQuadrant.getSide().negateIfLeftSide(-20.0);
   QuadrupedSupportPolygon extreme = createExtremeFootPolygon(robotQuadrant, new Point3D(negateIfLeftSide, negateIfLeftSide, 0.0));
   assertTrue("not cross", QuadrupedSupportPolygonTools.areLegsCrossing(extreme));
   extreme.removeFootstep(robotQuadrant.getSameSideQuadrant());
   assertTrue("not cross", QuadrupedSupportPolygonTools.areLegsCrossing(extreme));
 }
}

代码示例来源：origin: us.ihmc/IHMCHumanoidBehaviors

private void sequenceSquareUp()
{
 FootstepDataListMessage footstepDataList = new FootstepDataListMessage(swingTime.getDoubleValue(), transferTime.getDoubleValue());
 FramePose footstepPose = new FramePose();
 RobotSide robotSide = activeSideForFootControl.getEnumValue();
 if (robotSide == null)
   System.out.println("choose a foot to be squared up");
 else
 {
   footstepPose.setToZero(fullRobotModel.getEndEffectorFrame(robotSide.getOppositeSide(), LimbName.LEG));
   footstepPose.setY(robotSide.getOppositeSide().negateIfLeftSide(walkingControllerParameters.getInPlaceWidth()));
   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));
 }
}

代码示例来源：origin: us.ihmc/ihmc-humanoid-robotics

public static FramePoint3D getCenterOfPredictedContactPointsInFootstep(Footstep footstep, RobotSide side, double centimetersForwardFromCenter, double centimetersInFromCenter)
{
 Point2D footstepCenter;
 List<Point2D> predictedContactPoints = footstep.getPredictedContactPoints();
 if (predictedContactPoints != null)
 {
   ConvexPolygon2D contactPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(predictedContactPoints));
   footstepCenter = new Point2D(contactPolygon.getCentroid());
 }
 else
 {
   footstepCenter = new Point2D();
 }
 footstepCenter.setX(footstepCenter.getX() + centimetersForwardFromCenter);
 footstepCenter.setY(footstepCenter.getY() + side.negateIfLeftSide(centimetersInFromCenter));
 FramePoint3D footstepCenter3d = new FramePoint3D(footstep.getSoleReferenceFrame(), footstepCenter.getX(), footstepCenter.getY(), 0.0);
 footstepCenter3d.changeFrame(worldFrame);
 return footstepCenter3d;
}

代码示例来源：origin: us.ihmc/ihmc-humanoid-behaviors

private void sequenceSquareUp()
{
 FootstepDataListMessage footstepDataList = HumanoidMessageTools.createFootstepDataListMessage(swingTime.getDoubleValue(), transferTime.getDoubleValue());
 FramePose3D footstepPose = new FramePose3D();
 RobotSide robotSide = activeSideForFootControl.getEnumValue();
 if (robotSide == null)
   System.out.println("choose a foot to be squared up");
 else
 {
   footstepPose.setToZero(fullRobotModel.getSoleFrame(robotSide.getOppositeSide()));
   footstepPose.setY(robotSide.getOppositeSide().negateIfLeftSide(walkingControllerParameters.getSteppingParameters().getInPlaceWidth()));
   footstepPose.changeFrame(worldFrame);
   Point3D footLocation = new Point3D(footstepPose.getPosition());
   Quaternion footOrientation = new Quaternion(footstepPose.getOrientation());
   FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(robotSide, footLocation, footOrientation);
   footstepDataList.getFootstepDataList().add().set(footstepData);
   pipeLine.submitSingleTaskStage(new FootstepListTask(footstepListBehavior, footstepDataList));
 }
}

代码示例来源：origin: us.ihmc/ihmc-avatar-interfaces

private void computeBestEffortPlan(double horizonLength)
{
 FramePose3D bodyGoalPose = new FramePose3D(mainObjective.getGoal().getGoalPoseBetweenFeet());
 FramePose3D bodyStartPose = new FramePose3D();
 RobotSide side = mainObjective.getInitialStanceFootSide() != null ? mainObjective.getInitialStanceFootSide() : RobotSide.LEFT;
 FramePose3D stanceFootPose = mainObjective.getInitialStanceFootPose();
 double defaultStepWidth = footstepPlanningParameters.getIdealFootstepWidth();
 ReferenceFrame stanceFrame = new PoseReferenceFrame("stanceFrame", stanceFootPose);
 FramePoint2D bodyStartPoint = new FramePoint2D(stanceFrame);
 bodyStartPoint.setY(side.negateIfLeftSide(defaultStepWidth / 2.0));
 bodyStartPoint.changeFrameAndProjectToXYPlane(ReferenceFrame.getWorldFrame());
 bodyStartPose.setPosition(bodyStartPoint.getX(), bodyStartPoint.getY(), 0.0);
 bodyStartPose.setOrientationYawPitchRoll(stanceFootPose.getYaw(), 0.0, 0.0);
 Vector2D goalDirection = new Vector2D(bodyGoalPose.getPosition());
 goalDirection.sub(bodyStartPose.getX(), bodyStartPose.getY());
 goalDirection.scale(horizonLength / goalDirection.length());
 Point3D waypoint = new Point3D(bodyStartPose.getPosition());
 waypoint.add(goalDirection.getX(), goalDirection.getY(), 0.0);
 waypointPlan.get().add(waypoint);
}

内容来源于网络，如有侵权，请联系作者删除！

RobotSide

相关文章

    查看更多
    微信公众号

    热门标签

    更多
    JavaquerypythonNode开发语言requestUtil数据库Table后端算法LoggerMessageElementParser

    最新文章

    更多
    • 蜀ICP备13028337号-1 大数据知识库 https://www.saoniuhuo.com © All rights reserved
    • 本站内容来源互联网,如果侵犯您的权益请联系我们删除, 联系方式：448109455@qq.com