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

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

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

RobotSide.negateIfRightSide介绍

暂无

代码示例

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

@Override
public void sendArmStraightConfiguration(double trajectoryDuration, RobotSide robotSide)
{
 double[] jointAngles = new double[7];
 int index = 0;
 jointAngles[index++] = -1.5; // shoulderPitch
 jointAngles[index++] = robotSide.negateIfRightSide(-1.4); // shoulderRoll
 jointAngles[index++] = 1.5; // shoulderYaw
 jointAngles[index++] = robotSide.negateIfRightSide(-0.5); // elbowPitch
 jointAngles[index++] = robotSide.negateIfRightSide(0.0); // forearmYaw
 jointAngles[index++] = robotSide.negateIfRightSide(0.0); // wristRoll
 jointAngles[index++] = 0.0; // wristPitch
 ArmTrajectoryMessage message = HumanoidMessageTools.createArmTrajectoryMessage(robotSide, 0.4, jointAngles);
 armTrajectoryPublisher.publish(message);
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

@Override
 protected void updateTransformToParent(RigidBodyTransform transformToParent)
 {
   transformToParent.setTranslation(new Vector3D(0.0, robotSide.negateIfRightSide(0.15), 0.0));
 }
};

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

public static List<ReachingManifoldMessage> createSphereManifoldMessagesForValkyrie(RobotSide robotSide, RigidBodyBasics hand,
                                          Tuple3DReadOnly manifoldOriginPosition, double radius)
{
 List<ReachingManifoldMessage> messages = new ArrayList<>();
 ReachingManifoldMessage message = HumanoidMessageTools.createReachingManifoldMessage(hand);
 ConfigurationSpaceName[] spaces = {ConfigurationSpaceName.YAW, ConfigurationSpaceName.ROLL, ConfigurationSpaceName.Y, ConfigurationSpaceName.PITCH};
 double[] lowerLimits = new double[] {-Math.PI, -0.5 * Math.PI, robotSide.negateIfRightSide(radius), -Math.PI};
 double[] upperLimits = new double[] {Math.PI, 0.5 * Math.PI, robotSide.negateIfRightSide(radius), Math.PI};
 message.getManifoldOriginPosition().set(manifoldOriginPosition);
 HumanoidMessageTools.packManifold(ConfigurationSpaceName.toBytes(spaces), lowerLimits, upperLimits, message);
 messages.add(message);
 return messages;
}

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

private void createFistContactPoints(RobotSide robotSide)
{
 String wrist = jointMap.getJointBeforeHandName(robotSide);
 Vector3D knuckleIndex1 = new Vector3D(-0.02, robotSide.negateIfRightSide(0.115), 0.03);
 Vector3D knuckleMiddle1 = new Vector3D(-0.02, robotSide.negateIfRightSide(0.11), -0.015);
 Vector3D knucklePinky1 = new Vector3D(-0.02, robotSide.negateIfRightSide(0.105), -0.06);
 addSimulationContactPoint(wrist, knuckleIndex1);
 addSimulationContactPoint(wrist, knuckleMiddle1);
 addSimulationContactPoint(wrist, knucklePinky1);
}

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

public static Point2D computeMidFootPoint(FootstepNode node, double idealStepWidth)
{
 double w = idealStepWidth / 2.0;
 double vx = node.getRobotSide().negateIfRightSide(Math.sin(node.getYaw()) * w);
 double vy = -node.getRobotSide().negateIfRightSide(Math.cos(node.getYaw()) * w);
 return new Point2D(node.getX() + vx, node.getY() + vy);
}

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

protected FramePoint2D offsetFootstepFromPath(RobotSide currentFootstepSide, FramePoint2D footstepPosition2d, double footHeading, double offsetAmount)
{
 double sideWaysHeading = footHeading + Math.PI / 2.0;
 FrameVector2D offsetVector = new FrameVector2D(WORLD_FRAME, Math.cos(sideWaysHeading), Math.sin(sideWaysHeading));
 offsetVector.scale(currentFootstepSide.negateIfRightSide(offsetAmount));
 FramePoint2D footstepPosition = new FramePoint2D(footstepPosition2d);
 footstepPosition.changeFrame(WORLD_FRAME);
 footstepPosition.add(offsetVector);
 return footstepPosition;
}

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

protected FramePoint2d offsetFootstepFromPath(RobotSide currentFootstepSide, FramePoint2d footstepPosition2d, double footHeading, double offsetAmount)
{
 double sideWaysHeading = footHeading + Math.PI / 2.0;
 FrameVector2d offsetVector = new FrameVector2d(WORLD_FRAME, Math.cos(sideWaysHeading), Math.sin(sideWaysHeading));
 offsetVector.scale(currentFootstepSide.negateIfRightSide(offsetAmount));
 FramePoint2d footstepPosition = new FramePoint2d(footstepPosition2d);
 footstepPosition.changeFrame(WORLD_FRAME);
 footstepPosition.add(offsetVector);
 return footstepPosition;
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

private Footstep createFootsteps(double length, double width, int numberOfSteps)
{
 ArrayList<Footstep> upcomingFootsteps = new ArrayList<>();
 RobotSide robotSide = RobotSide.LEFT;
 for (int i = 0; i < numberOfSteps; i++)
 {
   FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame());
   footstepPose.setPosition(length * (i + 1), robotSide.negateIfRightSide(0.5 * width), 0.0);
   upcomingFootsteps.add(new Footstep(robotSide, footstepPose, false));
   FramePoint2D referenceFootstepPosition = new FramePoint2D(footstepPose.getPosition());
   robotSide = robotSide.getOppositeSide();
 }
 return upcomingFootsteps.get(0);
}

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

public FramePose3D getPoseFromCylindricalCoordinates(RobotSide robotSide, ReferenceFrame frame, double radiansFromYAxis, double radius, double z,
                             double outwardRotation, double pitchRotation)
{
 getPosition(position, frame, radiansFromYAxis, radius, z);
 preRotation.setYawPitchRoll(0.0, Math.PI / 2.0, -Math.PI / 2.0);
 rotation.set(preRotation);
 rotation.appendRollRotation(robotSide.negateIfRightSide(Math.PI / 2.0) - radiansFromYAxis);
 rotation.appendYawRotation(robotSide.negateIfRightSide(outwardRotation));
 rotation.appendPitchRotation(pitchRotation);
 orientation.setIncludingFrame(frame, rotation);
 return new FramePose3D(position, orientation);
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

public void sendFootStepMessages(int numberOfFootstepsToPlan)
{
 RobotSide robotSide = RobotSide.LEFT;
 FramePoint3D footstepLocation = new FramePoint3D();
 FrameQuaternion footstepOrientation = new FrameQuaternion();
 upcomingFootstepsData.clear();
 for (int i = 1; i < numberOfFootstepsToPlan + 1; i++)
 {
   Footstep footstep = new Footstep(robotSide);
   footstepLocation.set(i * stepLength, robotSide.negateIfRightSide(stepWidth), 0.0);
   footstep.setPose(footstepLocation, footstepOrientation);
   FootstepTiming timing = new FootstepTiming(swingTime, transferTime);
   upcomingFootstepsData.add(new FootstepData(footstep, timing));
   robotSide = robotSide.getOppositeSide();
 }
 numberOfUpcomingFootsteps.set(upcomingFootstepsData.size());
}

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

public FramePose getPoseFromCylindricalCoordinates(RobotSide robotSide, ReferenceFrame frame, double radiansFromYAxis, double radius, double z,
                             double outwardRotation, double pitchRotation)
{
 getPosition(position, frame, radiansFromYAxis, radius, z);
 RotationTools.convertYawPitchRollToMatrix(0.0, Math.PI / 2.0, -Math.PI / 2.0, preRotation);
 rotX.rotX(robotSide.negateIfRightSide(Math.PI / 2.0) - radiansFromYAxis);
 rotZ.rotZ(robotSide.negateIfRightSide(outwardRotation));
 rotY.rotY(pitchRotation);
 rotation.mul(preRotation, rotX);
 rotation.mul(rotZ);
 rotation.mul(rotY);
 orientation.setIncludingFrame(frame, rotation);
 return new FramePose(position, orientation);
}

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

/**
* Adds offsets to the current swing foot position to find potential neighbors
* @param xOffset
* @param yOffset
* @param yawOffset
*/
public void addOffset(double xOffset, double yOffset, double yawOffset)
{
 /** Based on stance foot side, step width sign would change*/
 double ySign = stepSide.negateIfRightSide(1.0);
 FramePose3D step = new FramePose3D(stanceFrame);
 step.setX(xOffset);
 step.setY(ySign * yOffset);
 step.changeFrame(worldFrame);
 neighbors.add(new FootstepNode(step.getX(), step.getY(), yawStanceFoot + ySign * yawOffset, stepSide));
}

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

private void sendFootstepCommand(double stepLength, int numberOfFootsteps)
{
 FootstepDataListMessage footstepMessage = HumanoidMessageTools.createFootstepDataListMessage(getSwingTime(), getTransferTime());
 RobotSide side = RobotSide.LEFT;
 Quaternion orientation = new Quaternion();
 for (int i = 1; i < numberOfFootsteps; i++)
 {
   addFootstep(new Point3D(i * stepLength, side.negateIfRightSide(getStepWidth() / 2.0), 0.0), orientation, side, footstepMessage);
   side = side.getOppositeSide();
 }
 addFootstep(new Point3D((numberOfFootsteps - 1) * stepLength, side.negateIfRightSide(getStepWidth() / 2.0), 0.0), orientation, side, footstepMessage);
 footstepMessage.setFinalTransferDuration(getFinalTransferDuration());
 drcSimulationTestHelper.publishToController(footstepMessage);
}

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

private void computeDistanceAndAngleToDestination(ReferenceFrame supportAnkleZUpFrame, RobotSide swingLegSide, FramePoint2d desiredDestination)
{
 FramePoint2d destinationInAnkleFrame = new FramePoint2d(desiredDestination);
 destinationInAnkleFrame.changeFrame(supportAnkleZUpFrame);
 FramePoint2d squaredUpMidpointInAnkleFrame = new FramePoint2d(supportAnkleZUpFrame, 0.0,
    swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue() / 2.0));
 FrameVector2d midpointToDestination = new FrameVector2d(destinationInAnkleFrame);
 midpointToDestination.sub(squaredUpMidpointInAnkleFrame);
 distanceToDestination.set(midpointToDestination.length());
 angleToDestination.set(Math.atan2(midpointToDestination.getY(), midpointToDestination.getX()));
}

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

@Override
  public void variableChanged(YoVariable<?> v)
  {
   for (RobotSide robotSide : RobotSide.values)
   {
     OneDoFJoint hipYawJoint = fullRobotModel.getLegJoint(robotSide, LegJointName.HIP_YAW);
     ReferenceFrame frameBeforeJoint = hipYawJoint.getFrameBeforeJoint();
     RigidBodyTransform postCorruptionTransform = new RigidBodyTransform();
     Vector3d offsetVector = hipYawOffset.getVector3dCopy();
     offsetVector.setY(robotSide.negateIfRightSide(offsetVector.getY()));
     postCorruptionTransform.setTranslation(offsetVector);
     frameBeforeJoint.corruptTransformToParentPostMultiply(postCorruptionTransform);
   }
  }
};

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

public void submitHumanoidArmPose(RobotSide robotSide, HumanoidArmPose armPose)
{
 FrameOrientation desiredUpperArmOrientation = new FrameOrientation(fullRobotModel.getChest().getBodyFixedFrame(),
    armPose.getDesiredUpperArmYawPitchRoll());
 double elbowAngle = robotSide.negateIfRightSide(armPose.getDesiredElbowAngle(robotSide));
 double[] handOrientation = new double[3];
 if (enableHandOrientation.getBooleanValue())
 {
   handOrientation = armPose.getDesiredHandYawPitchRoll();
 }
 FrameOrientation desiredHandOrientation = new FrameOrientation(lowerArmsFrames.get(robotSide), handOrientation);
 submitHandPose(robotSide, desiredUpperArmOrientation, elbowAngle, desiredHandOrientation, true);
}

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

private RobotSide computeStanceFootPose(FramePose3D startPose, FootstepPlannerParameters parameters, FramePose3D stancePoseToPack)
{
 RobotSide side = RobotSide.LEFT;
 double stanceWidth = parameters.getIdealFootstepWidth();
 ReferenceFrame bodyFrame = new PoseReferenceFrame("stanceFrame", startPose);
 FramePoint3D footPosition = new FramePoint3D(bodyFrame);
 footPosition.setY(side.negateIfRightSide(stanceWidth / 2.0));
 footPosition.changeFrame(ReferenceFrame.getWorldFrame());
 stancePoseToPack.setToZero(ReferenceFrame.getWorldFrame());
 stancePoseToPack.setPosition(footPosition);
 stancePoseToPack.setOrientation(startPose.getOrientation());
 return side;
}

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

public void submitHumanoidArmPose(RobotSide robotSide, HumanoidArmPose armPose)
{
 FrameQuaternion desiredUpperArmOrientation = new FrameQuaternion();
 desiredUpperArmOrientation.setYawPitchRollIncludingFrame(fullRobotModel.getChest().getBodyFixedFrame(), armPose.getDesiredUpperArmYawPitchRoll());
 double elbowAngle = robotSide.negateIfRightSide(armPose.getDesiredElbowAngle(robotSide));
 double[] handOrientation = new double[3];
 if (enableHandOrientation.getBooleanValue())
 {
   handOrientation = armPose.getDesiredHandYawPitchRoll();
 }
 FrameQuaternion desiredHandOrientation = new FrameQuaternion();
 desiredHandOrientation.setYawPitchRollIncludingFrame(lowerArmsFrames.get(robotSide), handOrientation);
 submitHandPose(robotSide, desiredUpperArmOrientation, elbowAngle, desiredHandOrientation, true);
}

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

@Override
public final void setGoal(FootstepPlannerGoal goal)
{
 checkGoalType(goal);
 FramePose3D goalPose = goal.getGoalPoseBetweenFeet();
 ReferenceFrame goalFrame = new PoseReferenceFrame("GoalFrame", goalPose);
 for (RobotSide side : RobotSide.values)
 {
   FramePose3D goalNodePose = new FramePose3D(goalFrame);
   goalNodePose.setY(side.negateIfRightSide(parameters.getIdealFootstepWidth() / 2.0));
   goalNodePose.changeFrame(goalPose.getReferenceFrame());
   FootstepNode goalNode = new FootstepNode(goalNodePose.getX(), goalNodePose.getY(), goalNodePose.getYaw(), side);
   goalNodes.put(side, goalNode);
 }
 nodeExpansion.setGoalNodes(goalNodes);
}

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

private void computeForwardFootstepList()
{
 forwardFootstepList.getFootstepDataList().clear();
 RobotSide swingSide = initialSwingSide.getEnumValue();
 for (int i = 0; i < numberOfStepsToTake.getIntegerValue(); i++)
 {
   FootstepDataMessage footstepDataMessage = constructFootstepDataMessage(midFootZUpFrame, footstepLength.getDoubleValue() * (i + 1),
                                      0.5 * swingSide.negateIfRightSide(footstepWidth.getDoubleValue()), swingSide);
   forwardFootstepList.getFootstepDataList().add().set(footstepDataMessage);
   swingSide = swingSide.getOppositeSide();
 }
 forwardFootstepList.setDefaultSwingDuration(swingTime.getDoubleValue());
 forwardFootstepList.setDefaultTransferDuration(transferTime.getDoubleValue());
}

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

RobotSide

相关文章

    查看更多
    微信公众号

    热门标签

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

    最新文章

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