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

FloatingJointBasics.getJointPose介绍

暂无

代码示例

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

/** {@inheritDoc} */
@Override
default void setJointPosition(Tuple3DReadOnly jointTranslation)
{
 getJointPose().setPosition(jointTranslation);
}

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

/** {@inheritDoc} */
@Override
default void setJointOrientation(Orientation3DReadOnly jointOrientation)
{
 getJointPose().setOrientation(jointOrientation);
}

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

/** {@inheritDoc} */
@Override
default void setJointConfigurationToZero()
{
 getJointPose().setToZero();
}

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

/**
* Integrates the given {@code joint}'s velocity to update its configuration.
* 
* @param joint the joint to integrate the state of. The joint configuration is modified.
*/
public void integrateFromVelocity(FloatingJointBasics joint)
{
 integrate(joint.getJointTwist(), joint.getJointPose());
}

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

private RobotConfigurationData extractRobotConfigurationData(Pair<FloatingJointBasics, OneDoFJointBasics[]> initialFullRobotModel)
  {
   OneDoFJointBasics[] joints = initialFullRobotModel.getRight();
   RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(joints, new ForceSensorDefinition[0], new IMUDefinition[0]);
   RobotConfigurationDataFactory.packJointState(robotConfigurationData, Arrays.stream(joints).collect(Collectors.toList()));

   FloatingJointBasics rootJoint = initialFullRobotModel.getLeft();
   if (rootJoint != null)
   {
     robotConfigurationData.getRootTranslation().set(rootJoint.getJointPose().getPosition());
     robotConfigurationData.getRootOrientation().set(rootJoint.getJointPose().getOrientation());
   }
   return robotConfigurationData;
  }
}

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

/**
* Integrates the given {@code joint}'s acceleration and velocity to update its velocity and
* configuration.
* 
* @param joint the joint to integrate the state of. The joint configuration is modified.
*/
public void doubleIntegrateFromAcceleration(FloatingJointBasics joint)
{
 doubleIntegrate(joint.getJointAcceleration(), joint.getJointTwist(), joint.getJointPose());
}

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

public void recordCurrentState()
{
 FloatingJointBasics rootJoint = fullRobotModel.getRootJoint();
 
 rootJointTranslation.set(rootJoint.getJointPose().getPosition());
 rootJointRotation.set(rootJoint.getJointPose().getOrientation());
 yoRootJointTranslation.set(rootJointTranslation);
 yoRootJointRotation.set(rootJointRotation);
}

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

public static RobotConfigurationData extractRobotConfigurationData(FullHumanoidRobotModel fullRobotModel)
{
 OneDoFJointBasics[] joints = FullRobotModelUtils.getAllJointsExcludingHands(fullRobotModel);
 RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(joints, new ForceSensorDefinition[0], new IMUDefinition[0]);
 RobotConfigurationDataFactory.packJointState(robotConfigurationData, Arrays.stream(joints).collect(Collectors.toList()));
 robotConfigurationData.getRootTranslation().set(fullRobotModel.getRootJoint().getJointPose().getPosition());
 robotConfigurationData.getRootOrientation().set(fullRobotModel.getRootJoint().getJointPose().getOrientation());
 return robotConfigurationData;
}

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

public void updateFullRobotModel(KinematicsToolboxOutputStatus solution)
{
 if (jointsHashCode != solution.getJointNameHash())
   throw new RuntimeException("Hashes are different.");
 for (int i = 0; i < oneDoFJoints.length; i++)
 {
   float q = solution.getDesiredJointAngles().get(i);
   OneDoFJointBasics joint = oneDoFJoints[i];
   joint.setQ(q);
 }
 Vector3D translation = solution.getDesiredRootTranslation();
 rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ());
 Quaternion orientation = solution.getDesiredRootOrientation();
 rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS());
 fullRobotModelToUseForConversion.updateFrames();
}

代码示例来源：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-humanoid-robotics

privilegedRootJointPosition.set(rootJoint.getJointPose().getPosition());
Quaternion privilegedRootJointOrientation = new Quaternion();
privilegedRootJointOrientation.set(rootJoint.getJointPose().getOrientation());

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

@Override
public void update(long timestamp)
{
 fullRobot.updateFrames();
 latestTimestamp = timestamp;
 
 if(rootJoint != null)
 {
   robot.setOrientation(rootJoint.getJointPose().getOrientation());
   robot.setPositionInWorld(rootJoint.getJointPose().getPosition());
 }
 
 for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJointBasics> jointPair : revoluteJoints)
 {
   OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft();
   OneDoFJointBasics revoluteJoint = jointPair.getRight();
   pinJoint.setQ(revoluteJoint.getQ());
   pinJoint.setQd(revoluteJoint.getQd());
   pinJoint.setTau(revoluteJoint.getTau());
 }
 robot.setTime(robot.getTime() + updateDT);
 if (scs != null)
 {
   scs.tickAndUpdate();
 }
}

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

rootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ());
Quaternion orientation = robotConfigurationData.getRootOrientation();
rootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS());
rootJoint.getPredecessor().updateFramesRecursively();

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

desiredRootJoint.getJointPose().setPosition(translation.getX(), translation.getY(), translation.getZ());
Quaternion orientation = robotConfigurationData.getRootOrientation();
desiredRootJoint.getJointPose().getOrientation().setQuaternion(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getS());
desiredRootJoint.setJointVelocity(0, new DenseMatrix64F(6, 1));

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

@Override
public void receivedPacket(RobotConfigurationData packet)
{
 latestRobotConfigurationData = packet;
 FloatingJointBasics rootJoint = fullRobotModel.getRootJoint();
 TFloatArrayList newJointAngles = packet.getJointAngles();
 TFloatArrayList newJointVelocities = packet.getJointAngles();
 TFloatArrayList newJointTorques = packet.getJointTorques();
 OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints();
 for (int i = 0; i < newJointAngles.size(); i++)
 {
   oneDoFJoints[i].setQ(newJointAngles.get(i));
   oneDoFJoints[i].setQd(newJointVelocities.get(i));
   oneDoFJoints[i].setTau(newJointTorques.get(i));
 }
 pelvisTranslationFromRobotConfigurationData.set(packet.getRootTranslation());
 pelvisOrientationFromRobotConfigurationData.set(packet.getRootOrientation());
 rootJoint.getJointPose().setPosition(pelvisTranslationFromRobotConfigurationData.getX(), pelvisTranslationFromRobotConfigurationData.getY(), pelvisTranslationFromRobotConfigurationData.getZ());
 rootJoint.getJointPose().getOrientation().setQuaternion(pelvisOrientationFromRobotConfigurationData.getX(), pelvisOrientationFromRobotConfigurationData.getY(), pelvisOrientationFromRobotConfigurationData.getZ(), pelvisOrientationFromRobotConfigurationData.getS());
 
 computeDriftTransform();
 rootJoint.getPredecessor().updateFramesRecursively();
 yoVariableServer.update(System.currentTimeMillis());
}

代码示例来源：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

PrintTools.debug("QuadTree has changed, sending packet");
Point3D rootJointPosition = new Point3D();
rootJointPosition.set(rootJoint.getJointPose().getPosition());
robotPosition2d.set(rootJointPosition.getX(), rootJointPosition.getY());
reportMessage(HeightQuadTreeMessageConverter.convertQuadTreeForGround(quadTree, robotPosition2d, quadTreeMessageMaxRadius));

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

Pose3DReadOnly initialPose = new Pose3D(controllerRootJoint.getJointPose());
Pose3D aboveInitialPose = new Pose3D(initialPose);
aboveInitialPose.prependTranslation(0.0, 0.0, 0.1);
double desiredHeight = controllerRootJoint.getJointPose().getZ() - 0.2;
drcSimulationTestHelper.publishToController(HumanoidMessageTools.createPelvisHeightTrajectoryMessage(0.5, desiredHeight, worldFrame, worldFrame));

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

position.set(rootJoint.getJointPose().getPosition());
linearVelocity.set(rootJoint.getJointTwist().getLinearPart());