us.ihmc.robotics.geometry.GeometryTools.rotatePoseAboutAxis()方法的使用及代码示例

x33g5p2x  于2022-01-20 转载在 其他  
字(9.7k)|赞(0)|评价(0)|浏览(73)

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

GeometryTools.rotatePoseAboutAxis介绍

暂无

代码示例

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

public static void rotatePoseAboutAxis(ReferenceFrame rotationAxisFrame, Axis rotationAxis, double angle, FramePose3D framePoseToPack)
{
 GeometryTools.rotatePoseAboutAxis(rotationAxisFrame, rotationAxis, angle, false, false, framePoseToPack);
}

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

public static void rotatePoseAboutAxis(FrameVector3D rotatationAxis, FramePoint3D rotationAxisOrigin, double angle, FramePose3D framePoseToPack)
  {
   ReferenceFrame frameWhoseZAxisIsRotationAxis = constructReferenceFrameFromPointAndZAxis("rotationAxisFrame", rotationAxisOrigin,
                                                       rotatationAxis);
     rotatePoseAboutAxis(frameWhoseZAxisIsRotationAxis, Axis.Z, angle, framePoseToPack);
  }
}

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

public void addCrossBar()
{
 double height = 2.0 * steeringWheelRadius;
 double radius = 0.015;
 double heightAboveWheel = 0.1;
 
 FramePose3D crossBar = new FramePose3D(steeringWheelFrame);
 GeometryTools.rotatePoseAboutAxis(steeringWheelFrame, Axis.X, Math.PI / 2.0, crossBar);
 GeometryTools.rotatePoseAboutAxis(steeringWheelFrame, Axis.Z, Math.PI / 2.0, crossBar);
 crossBar.setPosition(new Vector3D(-height/2.0, 0.0, heightAboveWheel));
 
 RigidBodyTransform transform = new RigidBodyTransform();
 crossBar.get(transform);
 
 FrameCylinder3d spinnerHandleCylinder = new FrameCylinder3d(steeringWheelFrame, transform, height, radius);
 spokesCylinders.add(spinnerHandleCylinder);
 
 steeringWheelLinkGraphics.transform(transform);
 steeringWheelLinkGraphics.addCylinder(height, radius, YoAppearance.IndianRed());
 transform.invert();
 steeringWheelLinkGraphics.transform(transform);
 steeringWheelLink.setLinkGraphics(steeringWheelLinkGraphics);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testRotateAndUnrotatePoseAboutCollinearAxis()
{
 ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
 double angleToRotate = Math.toRadians(90.0);
 FramePose3D framePose = new FramePose3D(worldFrame);
 framePose.setPosition(0.0, 0.0, 1.0);
 FramePose3D framePoseCopy = new FramePose3D(framePose);
 GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, 0.5 * angleToRotate , framePose);
 GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, -0.5 * angleToRotate, framePose);
 double positionDistance = framePose.getPositionDistance(framePoseCopy);
 double orientationDistance = framePose.getOrientationDistance(framePoseCopy);
 assertTrue("Reference Frame shoud not have changed.  Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: "
    + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame);
 assertEquals("Change in FramePose Position after rotation is wrong.", 0.0, positionDistance, 1e-3);
 assertEquals("Change in FramePose Orientation after rotation is wrong.", 0.0, orientationDistance, Math.toRadians(0.1));
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testRotatePoseAboutCollinearAxisIncrementally()
{
 Random random = new Random(1179L);
 ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
 double angleToRotate = RandomNumbers.nextDouble(random, Math.toRadians(720.0));
 FramePose3D framePose = new FramePose3D(worldFrame);
 framePose.setPosition(0.0, 0.0, 1.0);
 FramePose3D framePoseCopy = new FramePose3D(framePose);
 GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, 0.5 * angleToRotate, framePose);
 GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, 0.5 * angleToRotate, framePose);
 double positionDistance = framePose.getPositionDistance(framePoseCopy);
 double orientationDistance = AngleTools.trimAngleMinusPiToPi(framePose.getOrientationDistance(framePoseCopy));
 double desiredOrientationDistance = AngleTools.trimAngleMinusPiToPi(angleToRotate);
 assertTrue("Reference Frame shoud not have changed.  Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: "
    + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame);
 assertEquals("Change in FramePose Position after rotation is wrong.", 0.0, positionDistance, 1e-3);
 assertEquals("Change in FramePose Orientation after rotation is wrong.", desiredOrientationDistance, orientationDistance, Math.toRadians(0.1));
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testRotatePoseAboutZAxisAndCheckOrientation()
{
 Random random = new Random(1179L);
 ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
 FramePose3D initialPose = new FramePose3D(worldFrame);
 FramePose3D rotatedPose = new FramePose3D(worldFrame);
 double angleToRotate = Math.toRadians(-720.01);
 AxisAngle desiredRotationAxisAngle = new AxisAngle(0.0, 0.0, 1.0, angleToRotate);
 while (angleToRotate < Math.toRadians(720.0))
 {
   initialPose.setPosition(0.0, 0.0, RandomNumbers.nextDouble(random, 10.0));
   rotatedPose.setIncludingFrame(initialPose);
   desiredRotationAxisAngle.setAngle(angleToRotate);
   GeometryTools.rotatePoseAboutAxis(rotatedPose.getReferenceFrame(), Axis.Z, angleToRotate, rotatedPose);
   PoseReferenceFrame initialPoseFrame = new PoseReferenceFrame("initialPoseFrame", initialPose);
   rotatedPose.changeFrame(initialPoseFrame);
   AxisAngle actualRotationAxisAngle = new AxisAngle(rotatedPose.getOrientation());
   assertTrue("Actual rotation: " + actualRotationAxisAngle + " does not match desired: " + desiredRotationAxisAngle,
      RotationTools.axisAngleEpsilonEquals(desiredRotationAxisAngle, actualRotationAxisAngle, 1e-5,
         AxisAngleComparisonMode.IGNORE_FLIPPED_AXES_ROTATION_DIRECTION_AND_COMPLETE_ROTATIONS));
   angleToRotate += Math.toRadians(5.0);
 }
}

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

rotatedPose.setIncludingFrame(initialPose);
GeometryTools.rotatePoseAboutAxis(worldFrame, Axis.Z, angleToRotate, lockPosition, lockOrientation, rotatedPose);
actualPosition.set(rotatedPose.getPosition());

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testRotatePoseAboutCollinearAxisAndCheckTranslation()
{
 Random random = new Random(1179L);
 double angleToRotate = RandomNumbers.nextDouble(random, Math.toRadians(720.0));
 ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
 FramePose3D framePose = new FramePose3D(worldFrame);
 framePose.setPosition(1.0, 0.0, 1.0);
 framePose.setOrientation(RandomGeometry.nextQuaternion(random));
 GeometryTools.rotatePoseAboutAxis(framePose.getReferenceFrame(), Axis.Z, angleToRotate, framePose);
 Point3D actualPosePositionAfterRotation = new Point3D(framePose.getPosition());
 Point3D desiredPosition = new Point3D(Math.cos(angleToRotate), Math.sin(angleToRotate), 1.0);
 Vector3D positionError = new Vector3D();
 positionError.sub(desiredPosition, actualPosePositionAfterRotation);
 assertTrue("Reference Frame shoud not have changed.  Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: "
    + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame);
 assertEquals("FramePose Position after rotation is wrong.  Desired position: " + desiredPosition + ", actual position: "
    + actualPosePositionAfterRotation, 0.0, positionError.length(), 1e-3);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testRotatePoseAboutOffsetAxisAndCheckTranslation()
{
 Random random = new Random(1179L);
 double angleToRotate = RandomNumbers.nextDouble(random, Math.toRadians(720.0));
 ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
 FramePose3D framePose = new FramePose3D(worldFrame);
 framePose.setPosition(1.0, 0.0, 1.0);
 framePose.setOrientation(RandomGeometry.nextQuaternion(random));
 FrameVector3D rotationAxis = new FrameVector3D(worldFrame, 0.0, 0.0, 1.0);
 FramePoint3D rotationAxisOrigin = new FramePoint3D(worldFrame, 0.0, 0.0, 0.0);
 GeometryTools.rotatePoseAboutAxis(rotationAxis, rotationAxisOrigin, angleToRotate, framePose);
 Point3D actualPosePositionAfterRotation = new Point3D(framePose.getPosition());
 Point3D desiredPosition = new Point3D(Math.cos(angleToRotate), Math.sin(angleToRotate), 1.0);
 Vector3D positionError = new Vector3D();
 positionError.sub(desiredPosition, actualPosePositionAfterRotation);
 assertTrue("Reference Frame shoud not have changed.  Actual frame: " + framePose.getReferenceFrame().getName() + ", Desired frame: "
    + worldFrame.getName(), framePose.getReferenceFrame() == worldFrame);
 assertEquals("FramePose Position after rotation is wrong.  Desired position: " + desiredPosition + ", actual position: "
    + actualPosePositionAfterRotation, 0.0, positionError.length(), 1e-3);
}

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

@Override
  protected void setBehaviorInput()
  {
   publishTextToSpeack("rotate Valve");
   FramePose3D point = offsetPointFromValveInWorldFrame(0.0, valveRadius + valveRadiusfinalOffset, distanceFromValve, 1.5708, 1.5708, -3.14159);
   GeometryTools.rotatePoseAboutAxis(valvePose, Axis.Z, degrees, point);
   uiPositionCheckerPacketpublisher.publish(MessageTools.createUIPositionCheckerPacket(point.getPosition()));
   HandTrajectoryMessage handTrajectoryMessage = HumanoidMessageTools.createHandTrajectoryMessage(RobotSide.RIGHT, 2, point.getPosition(),
                                                   point.getOrientation(),
                                                   CommonReferenceFrameIds.CHEST_FRAME.getHashId());
   handTrajectoryMessage.getSe3Trajectory().getFrameInformation().setDataReferenceFrameId(MessageTools.toFrameId(worldFrame));
   atlasPrimitiveActions.rightHandTrajectoryBehavior.setInput(handTrajectoryMessage);
  }
};

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

GeometryTools.rotatePoseAboutAxis(rotatedPose.getReferenceFrame(), Axis.Z, angleToRotate, lockPosition, lockOrientation, rotatedPose);

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

GeometryTools

相关文章

    查看更多
    微信公众号

    热门标签

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

    最新文章

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