us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics.getFrameAfterJoint()方法的使用及代码示例

x33g5p2x  于2022-01-19 转载在 其他  
字(16.5k)|赞(0)|评价(0)|浏览(97)

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

FloatingJointBasics.getFrameAfterJoint介绍

暂无

代码示例

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

public void updateJointPositions_ID_to_SCS()
{
  if (scsFloatingJoint != null)
  {
   idFloatingJoint.getFrameAfterJoint().getTransformToDesiredFrame(transformToWorld, ReferenceFrame.getWorldFrame());
   scsFloatingJoint.setRotationAndTranslation(transformToWorld);
  }
  for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints)
  {
   OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint);
   scsJoint.setQ(idJoint.getQ());
  }
}

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

private void getFloatingJointStateFromSCS()
{
  FloatingJoint scsJoint = (FloatingJoint) robot.getRootJoints().get(0);
  RigidBodyTransform jointTransform3D = scsJoint.getJointTransform3D();
  floatingJoint.getJointPose().set(jointTransform3D);
  floatingJoint.getFrameAfterJoint().update();
  FrameVector3D linearVelocity = new FrameVector3D();
  scsJoint.getVelocity(linearVelocity);
  linearVelocity.changeFrame(floatingJoint.getFrameAfterJoint());
  floatingJoint.getJointTwist().set(scsJoint.getAngularVelocityInBody(), linearVelocity);
}

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

private void compareJointAccelerations(double epsilon)
{
  boolean areEqual = true;
  String errorMessage = "Joint accelerations are not equal\n";
  String jointTriggerErrorMessage = "";
  FloatingJoint scsFloatingJoint = (FloatingJoint) robot.getRootJoints().get(0);
  SpatialAcceleration expected = extractFromFloatingJoint(scsFloatingJoint, floatingJoint.getFrameBeforeJoint(), floatingJoint.getFrameAfterJoint());
  if (!floatingJoint.getJointAcceleration().epsilonEquals(expected, epsilon))
  {
   areEqual = false;
   jointTriggerErrorMessage = " floating-joint";
  }
  errorMessage += "Floating joint:\nExpected: " + expected + "\nActual  : " + floatingJoint.getJointAcceleration();
  for (OneDoFJointBasics joint : allOneDoFJoints)
  {
   String jointName = joint.getName();
   OneDegreeOfFreedomJoint scsJoint = (OneDegreeOfFreedomJoint) robot.getJoint(jointName);
   if (!EuclidCoreTools.epsilonEquals(scsJoint.getQDD(), joint.getQdd(), epsilon))
   {
     areEqual = false;
     jointTriggerErrorMessage += " " + jointName;
   }
   errorMessage += "\n" + jointName + ",\texpected: " + String.format(FORMAT, scsJoint.getQDD()) + ",\tactual: "
      + String.format(FORMAT, joint.getQdd()) + ", difference: " + String.format(FORMAT, Math.abs(scsJoint.getQDD() - joint.getQdd()));
  }
  if (!areEqual)
  {
   throw new RuntimeException(errorMessage + "\nJoint(s) triggering error:" + jointTriggerErrorMessage);
  }
}

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

public void setFullRobotModelRootJointVelocityAndAngularVelocityToMatchRobot(FloatingJointBasics sixDoFJoint, FloatingJoint floatingJoint)
{
 FrameVector3D angularVelocityFrameVector = new FrameVector3D();
 FrameVector3D linearVelocityFrameVector = new FrameVector3D();
 ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint();
 ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint();
 floatingJoint.getVelocity(linearVelocityFrameVector);
 linearVelocityFrameVector.changeFrame(bodyFrame);
 floatingJoint.getAngularVelocity(angularVelocityFrameVector, bodyFrame);
 Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, angularVelocityFrameVector, linearVelocityFrameVector);
 sixDoFJoint.setJointTwist(bodyTwist);
}

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

public void updateJointVelocities_SCS_to_ID()
{
  if (scsFloatingJoint != null)
  {
   ReferenceFrame elevatorFrame = idFloatingJoint.getFrameBeforeJoint();
   ReferenceFrame rootBodyFrame = idFloatingJoint.getFrameAfterJoint();
   scsFloatingJoint.getVelocity(linearVelocity);
   linearVelocity.changeFrame(rootBodyFrame);
   scsFloatingJoint.getAngularVelocity(angularVelocity, rootBodyFrame);
   rootJointTwist.setIncludingFrame(rootBodyFrame, elevatorFrame, rootBodyFrame, angularVelocity, linearVelocity);
   idFloatingJoint.setJointTwist(rootJointTwist);
  }
  for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints)
  {
   OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint);
   idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue());
  }
}

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

public void computePelvisTrajectoryMessage()
{
 checkIfDataHasBeenSet();
 Point3D desiredPosition = new Point3D();
 Quaternion desiredOrientation = new Quaternion();
 ReferenceFrame pelvisFrame = fullRobotModelToUseForConversion.getRootJoint().getFrameAfterJoint();
 FramePose3D desiredPelvisPose = new FramePose3D(pelvisFrame);
 desiredPelvisPose.changeFrame(worldFrame);
 desiredPelvisPose.get(desiredPosition, desiredOrientation);
 PelvisTrajectoryMessage pelvisTrajectoryMessage = HumanoidMessageTools.createPelvisTrajectoryMessage(trajectoryTime, desiredPosition, desiredOrientation);
 output.getPelvisTrajectoryMessage().set(pelvisTrajectoryMessage);
}

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

private void readAndUpdateRootJointAngularAndLinearVelocity()
{      
 ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint();
 ReferenceFrame pelvisFrame = rootJoint.getFrameAfterJoint();
 // Update base frames without updating all frames to transform velocity into pelvis
 elevatorFrame.update();
 pelvisFrame.update();
 
 FrameVector3D linearVelocity = robot.getRootJointVelocity();
 linearVelocity.changeFrame(pelvisFrame);
 FrameVector3D angularVelocity = robot.getRootJointAngularVelocityInRootJointFrame(pelvisFrame);
 angularVelocity.changeFrame(pelvisFrame);
 Twist bodyTwist = new Twist(pelvisFrame, elevatorFrame, pelvisFrame, angularVelocity, linearVelocity);
 rootJoint.setJointTwist(bodyTwist);
}

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

@Override
  public void handle(long now)
  {
   FramePoint3D rootJointPosition = new FramePoint3D(robotVisualizer.getFullRobotModel().getRootJoint().getFrameAfterJoint());
   rootJointPosition.changeFrame(ReferenceFrame.getWorldFrame());
   rootJointOffset.setX(rootJointPosition.getX());
   rootJointOffset.setY(rootJointPosition.getY());
   rootJointOffset.setZ(rootJointPosition.getZ());
  }
};

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

public void updateJointVelocities_ID_to_SCS()
{
  if (scsFloatingJoint != null)
  {
   ReferenceFrame rootBodyFrame = idFloatingJoint.getFrameAfterJoint();
   rootJointTwist.setIncludingFrame(idFloatingJoint.getJointTwist());
   linearVelocity.setIncludingFrame(rootJointTwist.getLinearPart());
   angularVelocity.setIncludingFrame(rootJointTwist.getAngularPart());
   linearVelocity.changeFrame(ReferenceFrame.getWorldFrame());
   angularVelocity.changeFrame(rootBodyFrame);
   scsFloatingJoint.setVelocity(linearVelocity);
   scsFloatingJoint.setAngularVelocityInBody(angularVelocity);
  }
  for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints)
  {
   OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint);
   scsJoint.setQd(idJoint.getQd());
  }
}

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

public void setSixDoFJointAccelerationRandomly(FloatingJointBasics sixDoFJoint, Random random, double maxRootJointLinearAcceleration,
   double maxRootJointAngularAcceleration)
{
 // Note: To get the acceleration, you can't just changeFrame on the acceleration provided by SCS. Use setBasedOnOriginAcceleration instead.
 ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint();
 ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint();
 Twist bodyTwist = new Twist();
 bodyTwist.setIncludingFrame(sixDoFJoint.getJointTwist());
 FrameVector3D originAcceleration = new FrameVector3D(elevatorFrame, RandomGeometry.nextVector3D(random, maxRootJointLinearAcceleration));
 FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame, RandomGeometry.nextVector3D(random, maxRootJointAngularAcceleration));
 //      originAcceleration.changeFrame(elevatorFrame);
 SpatialAcceleration spatialAccelerationVector = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame);
 spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist);
 sixDoFJoint.setJointAcceleration(spatialAccelerationVector);
}

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

public void copyAccelerationFromForwardToInverseBroken(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint)
{
 ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint();
 ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint();
 FrameVector3D angularAccelerationInBody = new FrameVector3D();
 floatingJoint.getAngularAcceleration(angularAccelerationInBody, bodyFrame);
 FrameVector3D linearAccelerationInBody = new FrameVector3D();
 floatingJoint.getLinearAcceleration(linearAccelerationInBody);
 linearAccelerationInBody.changeFrame(bodyFrame);
 SpatialAcceleration jointAcceleration = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame);
 jointAcceleration.getLinearPart().set(linearAccelerationInBody);
 jointAcceleration.getAngularPart().set(angularAccelerationInBody);
 sixDoFJoint.setJointAcceleration(jointAcceleration);
}

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

public void copyAccelerationFromForwardToInverse(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint)
{
 // Note: To get the acceleration, you can't just changeFrame on the acceleration provided by SCS. Use setBasedOnOriginAcceleration instead.
 ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint();
 ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint();
 Twist bodyTwist = new Twist();
 bodyTwist.setIncludingFrame(sixDoFJoint.getJointTwist());
 FrameVector3D originAcceleration = new FrameVector3D(elevatorFrame);
 FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame);
 floatingJoint.getLinearAccelerationInWorld(originAcceleration);
 floatingJoint.getAngularAccelerationInBody(angularAcceleration);
 originAcceleration.changeFrame(elevatorFrame);
 SpatialAcceleration spatialAccelerationVector = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame);
 spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist);
 sixDoFJoint.setJointAcceleration(spatialAccelerationVector);
}

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

@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);
 robotLocation.set(robotPose.getPosition());
 robotOrientation.set(robotPose.getOrientation());
 this.targetLocation.set(robotLocation);
 this.targetOrientation.set(robotOrientation);
}

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

private void submitDesiredPelvisHeight(boolean parallelize, double offsetHeight)
{
 ReferenceFrame frameAfterRootJoint = fullRobotModel.getRootJoint().getFrameAfterJoint();
 FramePoint3D desiredPelvisPosition = new FramePoint3D(frameAfterRootJoint);
 desiredPelvisPosition.setZ(offsetHeight);
 desiredPelvisPosition.changeFrame(worldFrame);
 PelvisHeightTrajectoryTask comHeightTask = new PelvisHeightTrajectoryTask(desiredPelvisPosition.getZ(), pelvisHeightTrajectoryBehavior,
                                      trajectoryTime.getDoubleValue());
 if (parallelize)
 {
   pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior, comHeightTask);
   pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior, createSleepTask(sleepTimeBetweenPoses.getDoubleValue()));
 }
 else
 {
   pipeLine.submitSingleTaskStage(comHeightTask);
   pipeLine.submitSingleTaskStage(createSleepTask(sleepTimeBetweenPoses.getDoubleValue()));
 }
}

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

public void setFullRobotModelStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity)
{
 FloatingJointBasics rootJoint = fullRobotModel.getRootJoint();
 ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint();
 ReferenceFrame bodyFrame = rootJoint.getFrameAfterJoint();
 Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity),
    RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity));
 rootJoint.setJointTwist(bodyTwist);
 rootJoint.setJointPosition(RandomGeometry.nextVector3D(random));
 double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0);
 double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0);
 double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0);
 rootJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll);
 ArrayList<OneDoFJointBasics> oneDoFJoints = new ArrayList<OneDoFJointBasics>();
 fullRobotModel.getOneDoFJoints(oneDoFJoints);
 for (OneDoFJointBasics oneDoFJoint : oneDoFJoints)
 {
   double lowerLimit = oneDoFJoint.getJointLimitLower();
   double upperLimit = oneDoFJoint.getJointLimitUpper();
   double delta = upperLimit - lowerLimit;
   lowerLimit = lowerLimit + 0.05 * delta;
   upperLimit = upperLimit - 0.05 * delta;
   oneDoFJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit));
   oneDoFJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity));
 }
}

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

FramePoint3D pelvisHeight = new FramePoint3D(fullRobotModel.getRootJoint().getFrameAfterJoint());
pelvisHeight.changeFrame(ReferenceFrame.getWorldFrame());
PelvisHeightTrajectoryMessage message = HumanoidMessageTools.createPelvisHeightTrajectoryMessage(0.5, pelvisHeight.getZ() + 0.1);

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

drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.01);
FramePoint3D pelvisHeight = new FramePoint3D(fullRobotModel.getRootJoint().getFrameAfterJoint());
pelvisHeight.changeFrame(ReferenceFrame.getWorldFrame());
PelvisHeightTrajectoryMessage message = HumanoidMessageTools.createPelvisHeightTrajectoryMessage(0.5, pelvisHeight.getZ() + 0.05);

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

public boolean checkFullRobotModelRootJointAccelerationmatchesRobot(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint, double epsilon)
{
 // Note: To get the acceleration, you can't just changeFrame on the acceleration provided by SCS. Use setBasedOnOriginAcceleration instead.
 ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint();
 ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint();
 Twist bodyTwist = new Twist();
 bodyTwist.setIncludingFrame(sixDoFJoint.getJointTwist());
 FrameVector3D originAcceleration = new FrameVector3D(elevatorFrame);
 FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame);
 floatingJoint.getLinearAccelerationInWorld(originAcceleration);
 floatingJoint.getAngularAccelerationInBody(angularAcceleration);
 originAcceleration.changeFrame(elevatorFrame);
 //TODO: These should be from the inverse dynamics to the SCS frames...
 originAcceleration.changeFrame(ReferenceFrame.getWorldFrame());
 computedRootJointLinearAcceleration.set(originAcceleration);
 computedRootJointAngularAcceleration.set(angularAcceleration);
 SpatialAcceleration spatialAccelerationVectorOfSimulatedRootJoint = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame);
 spatialAccelerationVectorOfSimulatedRootJoint.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist);
 SpatialAcceleration spatialAccelerationVectorOfInverseDynamicsRootJoint = new SpatialAcceleration();
 spatialAccelerationVectorOfInverseDynamicsRootJoint.setIncludingFrame(sixDoFJoint.getJointAcceleration());
 return spatialAccelerationVectorOfInverseDynamicsRootJoint.epsilonEquals(spatialAccelerationVectorOfInverseDynamicsRootJoint, epsilon);
}

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

private void submitDesiredPelvisPositionOffsetAndOrientation(boolean parallelize, double dx, double dy, double dz, double yaw, double pitch, double roll)
{
 ReferenceFrame frameAfterRootJoint = fullRobotModel.getRootJoint().getFrameAfterJoint();
 FramePose3D desiredPelvisPose = new FramePose3D(frameAfterRootJoint);
 desiredPelvisPose.setPosition(dx, dy, dz);
 desiredPelvisPose.setOrientationYawPitchRoll(yaw, pitch, roll);
 desiredPelvisPose.changeFrame(worldFrame);
 Point3D position = new Point3D();
 Quaternion orientation = new Quaternion();
 desiredPelvisPose.get(position, orientation);
 PelvisTrajectoryMessage message = HumanoidMessageTools.createPelvisTrajectoryMessage(trajectoryTime.getDoubleValue(), position, orientation);
 PelvisTrajectoryTask task = new PelvisTrajectoryTask(message, pelvisTrajectoryBehavior);
 if (parallelize)
 {
   pipeLine.submitTaskForPallelPipesStage(pelvisTrajectoryBehavior, task);
   pipeLine.submitTaskForPallelPipesStage(pelvisTrajectoryBehavior, createSleepTask(sleepTimeBetweenPoses.getDoubleValue()));
 }
 else
 {
   pipeLine.submitSingleTaskStage(task);
   pipeLine.submitSingleTaskStage(createSleepTask(sleepTimeBetweenPoses.getDoubleValue()));
 }
}

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

boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2.0); //2.0);
ReferenceFrame rootFrame = drcSimulationTestHelper.getControllerFullRobotModel().getRootJoint().getFrameAfterJoint();
FramePoint3D pelvisPosition = new FramePoint3D(rootFrame);
pelvisPosition.changeFrame(ReferenceFrame.getWorldFrame());

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

FloatingJointBasics

相关文章

    查看更多
    微信公众号

    热门标签

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

    最新文章

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