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

x33g5p2x  于2022-01-26 转载在 其他  
字(13.4k)|赞(0)|评价(0)|浏览(58)

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

OneDoFJointBasics.getJointLimitUpper介绍

暂无

代码示例

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

public JointLimit(OneDoFJointBasics oneDoFJoint)
{
 upperPositionLimit = oneDoFJoint.getJointLimitUpper();     
 lowerPositionLimit = oneDoFJoint.getJointLimitLower(); 
 rangeOfMotion = upperPositionLimit - lowerPositionLimit;
 
 softLimitPercentageOfFullRangeOfMotion = 1.0;
 softUpperPositionLimit = upperPositionLimit; 
 softLowerPositionLimit = lowerPositionLimit; 
 softLimitRangeOfMotion = softUpperPositionLimit - softLowerPositionLimit;
 
 velocityLimit = Double.POSITIVE_INFINITY;          
 torqueLimit = Double.POSITIVE_INFINITY;   
}

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

private double getRandomJointAngleInRange(Random random, OneDoFJointBasics joint)
{
 double jointLimitUpper = joint.getJointLimitUpper();
 double jointLimitLower = joint.getJointLimitLower();
 return RandomNumbers.nextDouble(random, jointLimitLower, jointLimitUpper);
}

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

private double[] generateRandomJointPositions(Random random, OneDoFJointBasics[] armJoints)
{
 double[] desiredJointPositions = new double[armJoints.length];
 for (int i = 0; i < armJoints.length; i++)
 {
   OneDoFJointBasics joint = armJoints[i];
   desiredJointPositions[i] = RandomNumbers.nextDouble(random, joint.getJointLimitLower(), joint.getJointLimitUpper());
 }
 return desiredJointPositions;
}

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

private static void setJointPositionToMidRange(OneDoFJointBasics joint)
{
 double jointLimitUpper = joint.getJointLimitUpper();
 double jointLimitLower = joint.getJointLimitLower();
 joint.setQ(0.5 * (jointLimitUpper + jointLimitLower));
}

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

protected double getRandomValidJointAngle(RobotSide side, ArmJointName armJointName, FullHumanoidRobotModel fullRobotModel)
{
 OneDoFJointBasics armJoint = fullRobotModel.getArmJoint(side, armJointName);
 if (armJoint != null)
 {
   double jointAngle = armJoint.getJointLimitLower() + (armJoint.getJointLimitUpper() - armJoint.getJointLimitLower()) * random.nextDouble();
   return jointAngle;
 }
 else
 {
   return 0.0;
 }
}

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

public static PlaybackPose createRandomPosePlaybackRobotPose(Random random, FullRobotModel fullRobotModel, double poseDelay, double trajectoryTime)
  {
   LinkedHashMap<OneDoFJointBasics, Double> pose = new LinkedHashMap<>();

   OneDoFJointBasics[] oneDoFJoints = fullRobotModel.getOneDoFJoints();
   for (OneDoFJointBasics joint : oneDoFJoints)
   {
     double jointLimitLower = joint.getJointLimitLower();
     double jointLimitUpper = joint.getJointLimitUpper();
          if (jointLimitLower < -Math.PI) jointLimitLower = -Math.PI;
     if (jointLimitUpper > Math.PI) jointLimitUpper = Math.PI;
     pose.put(joint, RandomNumbers.nextDouble(random, jointLimitLower, jointLimitUpper));
   }

   return new PlaybackPose(pose, poseDelay, trajectoryTime);
  }
}

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

private void introduceRandomPose()
{
 for (int i = 0; i < oneDoFJoints.length; i++)
 {
   double newQ = generateRandomDoubleInRange(oneDoFJoints[i].getJointLimitUpper(), oneDoFJoints[i].getJointLimitLower());
   oneDoFJoints[i].setQ(newQ);
 }
}

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

private void randomizeJointPositions(Random random, OneDoFJointBasics[] joints, double percentOfMotionRangeAllowed)
{
 for (OneDoFJointBasics joint : joints)
 {
   double jointLimitLower = joint.getJointLimitLower();
   double jointLimitUpper = joint.getJointLimitUpper();
   double rangeReduction = (1.0 - percentOfMotionRangeAllowed) * (jointLimitUpper - jointLimitLower);
   jointLimitLower += 0.5 * rangeReduction;
   jointLimitUpper -= 0.5 * rangeReduction;
   joint.setQ(RandomNumbers.nextDouble(random, jointLimitLower, jointLimitUpper));
 }
}

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

private void setupSliderForChest()
{
 int sliderChannel = 1;
 for (SpineJointName spineJointName : spineJointNames)
 {
   OneDegreeOfFreedomJoint spineSCSJoint = spineSCSJoints.get(spineJointName);
   OneDoFJointBasics spineIDJoint = fullRobotModel.getSpineJoint(spineJointName);
   sliderBoard.setSlider(sliderChannel++, spineSCSJoint.getQYoVariable(), spineIDJoint.getJointLimitLower(), spineIDJoint.getJointLimitUpper());
 }
 for (NeckJointName neckJointName : neckJointNames)
 {
   OneDegreeOfFreedomJoint neckSCSJoint = neckSCSJoints.get(neckJointName);
   OneDoFJointBasics neckIDJoint = fullRobotModel.getNeckJoint(neckJointName);
   sliderBoard.setSlider(sliderChannel++, neckSCSJoint.getQYoVariable(), neckIDJoint.getJointLimitLower(), neckIDJoint.getJointLimitUpper());
 }
}

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

private void randomizeJointPositions(Random random, Pair<FloatingJointBasics, OneDoFJointBasics[]> randomizedFullRobotModel,
                  double percentOfMotionRangeAllowed)
{
 for (OneDoFJointBasics joint : randomizedFullRobotModel.getRight())
 {
   double jointLimitLower = joint.getJointLimitLower();
   if (Double.isInfinite(jointLimitLower))
    jointLimitLower = -Math.PI;
   double jointLimitUpper = joint.getJointLimitUpper();
   if (Double.isInfinite(jointLimitUpper))
    jointLimitUpper = -Math.PI;
   double rangeReduction = (1.0 - percentOfMotionRangeAllowed) * (jointLimitUpper - jointLimitLower);
   jointLimitLower += 0.5 * rangeReduction;
   jointLimitUpper -= 0.5 * rangeReduction;
   joint.setQ(RandomNumbers.nextDouble(random, jointLimitLower, jointLimitUpper));
 }
 MultiBodySystemTools.getRootBody(randomizedFullRobotModel.getRight()[0].getPredecessor()).updateFramesRecursively();
}

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

private Pair<FloatingJointBasics, OneDoFJointBasics[]> createFullRobotModelAtInitialConfiguration()
{
 RobotDescription robotDescription = new KinematicsToolboxControllerTestRobots.SevenDoFArm();
 Pair<FloatingJointBasics, OneDoFJointBasics[]> fullRobotModel = KinematicsToolboxControllerTestRobots.createInverseDynamicsRobot(robotDescription);
 for (OneDoFJointBasics joint : fullRobotModel.getRight())
 {
   if (Double.isFinite(joint.getJointLimitLower()) && Double.isFinite(joint.getJointLimitUpper()))
    joint.setQ(0.5 * (joint.getJointLimitLower() + joint.getJointLimitUpper()));
 }
 MultiBodySystemTools.getRootBody(fullRobotModel.getRight()[0].getPredecessor()).updateFramesRecursively();
 return fullRobotModel;
}

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

private void applyJointAngleCorrection()
{
 for (int i = 0; i < oneDoFJoints.length; i++)
 {
   OneDoFJointBasics oneDoFJoint = oneDoFJoints[i];
   double newQ = oneDoFJoint.getQ() - jointAnglesCorrection.get(i, 0);
   if (limitJointAngles) 
    newQ = Math.min(oneDoFJoint.getJointLimitUpper(), Math.max(newQ, oneDoFJoint.getJointLimitLower()));
   oneDoFJoint.setQ(newQ);
   oneDoFJoint.getFrameAfterJoint().update();
 }
 
 if (stepListener != null)
 {
   stepListener.didAnInverseKinemticsStep(errorScalar);
 }
}

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

private void updateState(double[] parameters)
{
 for (int i = 0; i < parameters.length; i++)
 {
   OneDoFJointBasics oneDoFJoint = oneDoFJoints[i];
   // apply constraints to the input parameters
   double newQ = UtilAngle.bound(parameters[i]);
   if (Double.isNaN(newQ))
    continue;
   newQ = parameters[i] = MathTools.clamp(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper());
   oneDoFJoint.setQ(newQ);
   oneDoFJoint.getFrameAfterJoint().update();
 }
}

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

public void setJointLimits(OneDoFJointBasics joint)
{
 positionSoftUpperLimit = joint.getJointLimitUpper();
 positionSoftLowerLimit = joint.getJointLimitLower();
 velocityLimitUpper = joint.getVelocityLimitUpper();
 velocityLimitLower = joint.getVelocityLimitLower();
 torqueLimitUpper = joint.getEffortLimitUpper();
 torqueLimitLower = joint.getEffortLimitLower();
}

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

private void updateJointAngles()
{
 for (int i = 0; i < oneDoFJoints.length; i++)
 {
   double newQ = oneDoFJoints[i].getQ() + correction.get(i, 0);
   newQ = MathTools.clamp(newQ, oneDoFJoints[i].getJointLimitLower(), oneDoFJoints[i].getJointLimitUpper());
   oneDoFJoints[i].setQ(newQ);
   oneDoFJoints[i].getFrameAfterJoint().update();
 }
}

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

private void applyCorrection()
{
 for (int i = 0; i < oneDoFJoints.length; i++)
 {
   OneDoFJointBasics oneDoFJoint;
   if (useSeed)
   {
    oneDoFJoint = oneDoFJointsSeed[i];
   }
   else
   {
    oneDoFJoint = oneDoFJoints[i];
   }
   double newQ = oneDoFJoint.getQ() - correction.get(i, 0);
   newQ = MathTools.clamp(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper());
   oneDoFJoint.setQ(newQ);
   oneDoFJoint.getFrameAfterJoint().update();
 }
}

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

public static ArmTrajectoryMessage createArmMessage(FullHumanoidRobotModel fullRobotModel, RobotSide side)
  {
   RigidBodyBasics chest = fullRobotModel.getChest();
   RigidBodyBasics hand = fullRobotModel.getHand(side);
   OneDoFJointBasics[] joints = MultiBodySystemTools.createOneDoFJointPath(chest, hand);
   ArmTrajectoryMessage message = HumanoidMessageTools.createArmTrajectoryMessage(side);

   for (int jointIdx = 0; jointIdx < joints.length; jointIdx++)
   {
     OneDoFJointBasics joint = joints[jointIdx];
     double angle1 = MathTools.clamp(Math.toRadians(45.0), joint.getJointLimitLower() + 0.05, joint.getJointLimitUpper() - 0.05);
     double angle2 = MathTools.clamp(0.0, joint.getJointLimitLower() + 0.05, joint.getJointLimitUpper() - 0.05);
     OneDoFJointTrajectoryMessage jointTrajectoryMessage = message.getJointspaceTrajectory().getJointTrajectoryMessages().add();
     jointTrajectoryMessage.getTrajectoryPoints().add().set(HumanoidMessageTools.createTrajectoryPoint1DMessage(0.2, angle1, 0.0));
     jointTrajectoryMessage.getTrajectoryPoints().add().set(HumanoidMessageTools.createTrajectoryPoint1DMessage(0.4, angle2, 0.0));
   }
   return message;
  }
}

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

public void computeArmTrajectoryMessage(RobotSide robotSide)
{
 RigidBodyBasics hand = fullRobotModelToUseForConversion.getHand(robotSide);
 RigidBodyBasics chest = fullRobotModelToUseForConversion.getChest();
 OneDoFJointBasics[] armJoints = MultiBodySystemTools.createOneDoFJointPath(chest, hand);
 int numberOfArmJoints = armJoints.length;
 double[] desiredJointPositions = new double[numberOfArmJoints];
 for (int i = 0; i < numberOfArmJoints; i++)
 {
   OneDoFJointBasics armJoint = armJoints[i];
   desiredJointPositions[i] = MathTools.clamp(armJoint.getQ(), armJoint.getJointLimitLower(), armJoint.getJointLimitUpper());
 }
 ArmTrajectoryMessage armTrajectoryMessage = robotSide == RobotSide.LEFT ? output.getLeftArmTrajectoryMessage() : output.getRightArmTrajectoryMessage();
 armTrajectoryMessage.set(HumanoidMessageTools.createArmTrajectoryMessage(robotSide, trajectoryTime, desiredJointPositions));
}

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

private ArmTrajectoryMessage generateRandomArmTrajectoryMessage(Random random, int numberOfTrajectoryPoints, double trajectoryTime, RobotSide robotSide,
   OneDoFJointBasics[] armJoints)
{
 int numberOfJoints = armJoints.length;
 ArmTrajectoryMessage armTrajectoryMessage = HumanoidMessageTools.createArmTrajectoryMessage(robotSide);
 TrajectoryPoint1DCalculator trajectoryPoint1DCalculator = new TrajectoryPoint1DCalculator();
 for (int jointIndex = 0; jointIndex < numberOfJoints; jointIndex++)
 {
   OneDoFJointBasics joint = armJoints[jointIndex];
   OneDoFJointTrajectoryMessage jointTrajectoryMessage = armTrajectoryMessage.getJointspaceTrajectory().getJointTrajectoryMessages().add();
   trajectoryPoint1DCalculator.clear();
   for (int trajectoryPointIndex = 0; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++)
   {
    double desiredJointPosition = RandomNumbers.nextDouble(random, joint.getJointLimitLower(), joint.getJointLimitUpper());
    trajectoryPoint1DCalculator.appendTrajectoryPoint(desiredJointPosition);
   }
   trajectoryPoint1DCalculator.computeTrajectoryPointTimes(0.5, trajectoryTime + 0.5);
   trajectoryPoint1DCalculator.computeTrajectoryPointVelocities(true);
   SimpleTrajectoryPoint1DList trajectoryData = trajectoryPoint1DCalculator.getTrajectoryData();
   for (int trajectoryPointIndex = 0; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++)
   {
    SimpleTrajectoryPoint1D trajectoryPoint = trajectoryData.getTrajectoryPoint(trajectoryPointIndex);
    jointTrajectoryMessage.getTrajectoryPoints().add()
                        .set(HumanoidMessageTools.createTrajectoryPoint1DMessage(trajectoryPoint.getTime(),
                                                    trajectoryPoint.getPosition(),
                                                    trajectoryPoint.getVelocity()));
   }
 }
 return armTrajectoryMessage;
}

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

OneDoFJointBasics

相关文章

    查看更多
    微信公众号

    热门标签

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

    最新文章

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