us.ihmc.robotics.math.trajectories.waypoints.YoFrameEuclideanTrajectoryPoint类的使用及代码示例

x33g5p2x  于2022-02-05 转载在 其他  
字(14.6k)|赞(0)|评价(0)|浏览(122)

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

YoFrameEuclideanTrajectoryPoint介绍

暂无

代码示例

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

private void initializeSubTrajectory(int waypointIndex)
{
 int secondWaypointIndex = Math.min(waypointIndex + 1, numberOfWaypoints.getValue() - 1);
 YoFrameEuclideanTrajectoryPoint start = waypoints.get(waypointIndex);
 YoFrameEuclideanTrajectoryPoint end = waypoints.get(secondWaypointIndex);
 subTrajectory.setCubic(0.0, end.getTime() - start.getTime(), start.getPosition(), start.getLinearVelocity(), end.getPosition(), end.getLinearVelocity());
}

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

public void set(EuclideanTrajectoryPointInterface<?> euclideanTrajectoryPoint)
{
 frameWaypoint.setToZero(getReferenceFrame());
 frameWaypoint.set(euclideanTrajectoryPoint);
 getYoValuesFromFrameWaypoint();
}

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

private void appendWaypointUnsafe(double timeAtWaypoint, FramePoint3DReadOnly position, FrameVector3DReadOnly linearVelocity)
{
 waypoints.get(numberOfWaypoints.getIntegerValue()).set(timeAtWaypoint, position, linearVelocity);
 numberOfWaypoints.increment();
}

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

@Override
public void setToNaN(ReferenceFrame referenceFrame)
{
 switchCurrentReferenceFrame(referenceFrame);
 setToNaN();
}

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

@Override
public void setToNaN()
{
 setTimeToNaN();
 setPositionToNaN();
 setLinearVelocityToNaN();
}

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

YoFrameEuclideanTrajectoryPoint yoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame);
yoFrameEuclideanTrajectoryPoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
yoFrameEuclideanTrajectoryPoint.setIncludingFrame(worldFrame, simpleTrajectoryPoint);
yoFrameEuclideanTrajectoryPoint.getPosition(pointForVerification);
yoFrameEuclideanTrajectoryPoint.getLinearVelocity(linearVelocityForVerification);
assertEquals(time, yoFrameEuclideanTrajectoryPoint.getTime(), 1e-10);
assertTrue(pointForVerification.epsilonEquals(position, 1e-10));
assertTrue(linearVelocityForVerification.epsilonEquals(linearVelocity, 1e-10));
assertFalse(yoFrameEuclideanTrajectoryPoint.containsNaN());
yoFrameEuclideanTrajectoryPoint.setPositionToNaN();
assertTrue(yoFrameEuclideanTrajectoryPoint.containsNaN());
yoFrameEuclideanTrajectoryPoint.setPositionToZero();
assertFalse(yoFrameEuclideanTrajectoryPoint.containsNaN());
yoFrameEuclideanTrajectoryPoint.setLinearVelocityToNaN();
assertTrue(yoFrameEuclideanTrajectoryPoint.containsNaN());
yoFrameEuclideanTrajectoryPoint.setLinearVelocityToZero();
yoFrameEuclideanTrajectoryPoint.getPosition(position);
yoFrameEuclideanTrajectoryPoint.getLinearVelocity(linearVelocity);
assertFalse(Math.abs(yoFrameEuclideanTrajectoryPoint.getTime() - time) < 1e-7);
assertFalse(pointForVerification.epsilonEquals(position, 1e-7));
assertFalse(linearVelocityForVerification.epsilonEquals(linearVelocity, 1e-7));

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

String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameEuclideanTrajectoryPoint testedYoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint(expectedNamePrefix, expectedNameSuffix,
                                                      new YoVariableRegistry("schnoop"),
                                                      expectedFrame);
testedYoFrameEuclideanTrajectoryPoint.set(expectedTime, expectedPosition, expectedLinearVelocity);
testedYoFrameEuclideanTrajectoryPoint.setToNaN();
assertTrue(Double.isNaN(testedYoFrameEuclideanTrajectoryPoint.getTime()));
assertTrue(testedYoFrameEuclideanTrajectoryPoint.getPosition().containsNaN());
assertTrue(testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity().containsNaN());
testedYoFrameEuclideanTrajectoryPoint.registerReferenceFrame(expectedFrame);
expectedTime = RandomNumbers.nextDouble(random, 0.0, 1000.0);
expectedPosition = EuclidFrameRandomTools.nextFramePoint3D(random, worldFrame, 10.0, 10.0, 10.0);
expectedLinearVelocity = EuclidFrameRandomTools.nextFrameVector3D(random, worldFrame);
testedYoFrameEuclideanTrajectoryPoint.switchCurrentReferenceFrame(worldFrame);
testedYoFrameEuclideanTrajectoryPoint.registerReferenceFrame(worldFrame);
testedYoFrameEuclideanTrajectoryPoint.set(expectedTime, expectedPosition, expectedLinearVelocity);
testedYoFrameEuclideanTrajectoryPoint.setToNaN(expectedFrame);
assertTrue(expectedFrame == testedYoFrameEuclideanTrajectoryPoint.getReferenceFrame());
assertTrue(Double.isNaN(testedYoFrameEuclideanTrajectoryPoint.getTime()));
assertTrue(testedYoFrameEuclideanTrajectoryPoint.getPosition().containsNaN());
assertTrue(testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity().containsNaN());

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

YoFrameEuclideanTrajectoryPoint testedYoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint(expectedNamePrefix, expectedNameSuffix,
                                                      new YoVariableRegistry("schnoop"),
                                                      expectedFrame);
expectedLinearVelocity = EuclidFrameRandomTools.nextFrameVector3D(random, expectedFrame);
testedYoFrameEuclideanTrajectoryPoint.set(expectedTime, expectedPosition, expectedLinearVelocity);
expectedLinearVelocity = EuclidFrameRandomTools.nextFrameVector3D(random, expectedFrame);
testedYoFrameEuclideanTrajectoryPoint.set(expectedTime, expectedPosition, expectedLinearVelocity);
expectedLinearVelocity = EuclidFrameRandomTools.nextFrameVector3D(random, expectedFrame);
YoFrameEuclideanTrajectoryPoint expectedYoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint("sdfsd", "asd",
                                                       new YoVariableRegistry("asawe"),
                                                       expectedFrame);
testedYoFrameEuclideanTrajectoryPoint.set(expectedYoFrameEuclideanTrajectoryPoint);
assertTrue(expectedYoFrameEuclideanTrajectoryPoint.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint, epsilon));
assertWaypointContainsExpectedData(expectedNamePrefix, expectedNameSuffix, testedYoFrameEuclideanTrajectoryPoint.getReferenceFrame(),
                  testedYoFrameEuclideanTrajectoryPoint.getTime(), testedYoFrameEuclideanTrajectoryPoint.getPosition(),
                  testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(), testedYoFrameEuclideanTrajectoryPoint, epsilon);

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

YoFrameEuclideanTrajectoryPoint testedYoFrameEuclideanTrajectoryPoint, double epsilon)
assertTrue(expectedFrame == testedYoFrameEuclideanTrajectoryPoint.getReferenceFrame());
assertEquals(expectedTime, testedYoFrameEuclideanTrajectoryPoint.getTime(), epsilon);
assertEquals(expectedNamePrefix, testedYoFrameEuclideanTrajectoryPoint.getNamePrefix());
assertEquals(expectedNameSuffix, testedYoFrameEuclideanTrajectoryPoint.getNameSuffix());
assertTrue(expectedPosition.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint.getPosition(), epsilon));
assertTrue(expectedLinearVelocity.epsilonEquals(testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(), epsilon));
testedYoFrameEuclideanTrajectoryPoint.get(actualFrameEuclideanTrajectoryPoint);
FrameEuclideanTrajectoryPointTest.assertTrajectoryPointContainsExpectedData(expectedFrame, expectedTime, expectedPosition, expectedLinearVelocity,
                                      actualFrameEuclideanTrajectoryPoint, epsilon);
actualFrameEuclideanTrajectoryPoint = new FrameEuclideanTrajectoryPoint(expectedFrame);
testedYoFrameEuclideanTrajectoryPoint.get(actualFrameEuclideanTrajectoryPoint);
FrameEuclideanTrajectoryPointTest.assertTrajectoryPointContainsExpectedData(expectedFrame, expectedTime, expectedPosition, expectedLinearVelocity,
                                      actualFrameEuclideanTrajectoryPoint, epsilon);
Vector3D actualLinearVelocity = new Vector3D();
testedYoFrameEuclideanTrajectoryPoint.getPosition(actualPosition);
testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(actualLinearVelocity);
testedYoFrameEuclideanTrajectoryPoint.getPosition(actualFramePosition);
testedYoFrameEuclideanTrajectoryPoint.getLinearVelocity(actualFrameLinearVelocity);
testedYoFrameEuclideanTrajectoryPoint.getPositionIncludingFrame(actualFramePosition);
testedYoFrameEuclideanTrajectoryPoint.getLinearVelocityIncludingFrame(actualFrameLinearVelocity);

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

YoFrameEuclideanTrajectoryPoint yoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame);
yoFrameEuclideanTrajectoryPoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame());
FrameVector3D linearVelocity = new FrameVector3D(worldFrame, -0.4, 1.2, 3.3);
yoFrameEuclideanTrajectoryPoint.setTime(time);
yoFrameEuclideanTrajectoryPoint.setPosition(position);
yoFrameEuclideanTrajectoryPoint.setLinearVelocity(linearVelocity);
yoFrameEuclideanTrajectoryPoint.registerReferenceFrame(poseFrame);
yoFrameEuclideanTrajectoryPoint.changeFrame(poseFrame);
assertFalse(position.epsilonEquals(yoFrameEuclideanTrajectoryPoint.getPosition(), 1e-10));
assertFalse(linearVelocity.epsilonEquals(yoFrameEuclideanTrajectoryPoint.getLinearVelocity(), 1e-10));
assertTrue(position.epsilonEquals(yoFrameEuclideanTrajectoryPoint.getPosition(), 1e-10));
assertTrue(linearVelocity.epsilonEquals(yoFrameEuclideanTrajectoryPoint.getLinearVelocity(), 1e-10));
YoFrameEuclideanTrajectoryPoint yoFrameEuclideanTrajectoryPointTwo = new YoFrameEuclideanTrajectoryPoint(namePrefix, nameSuffix + "Two", registry,
                                                     poseFrame);
yoFrameEuclideanTrajectoryPointTwo.setTime(time);
yoFrameEuclideanTrajectoryPointTwo.setPosition(position);
yoFrameEuclideanTrajectoryPointTwo.setLinearVelocity(linearVelocity);
assertTrue(yoFrameEuclideanTrajectoryPointTwo.epsilonEquals(yoFrameEuclideanTrajectoryPointTwo, 1e-10));

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

poseFrame.setOrientationAndUpdate(poseOrientation);
YoFrameEuclideanTrajectoryPoint yoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame,
                                                   poseFrame);
SimpleEuclideanTrajectoryPoint simpleTrajectoryPoint = new SimpleEuclideanTrajectoryPoint();
yoFrameEuclideanTrajectoryPoint.setIncludingFrame(worldFrame, simpleTrajectoryPoint);
yoFrameEuclideanTrajectoryPoint.changeFrame(poseFrame);
YoFrameEuclideanTrajectoryPoint expectedYoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint(namePrefix, nameSuffix, registry,
                                                       poseFrame);
expectedYoFrameEuclideanTrajectoryPoint.setTime(time);
expectedYoFrameEuclideanTrajectoryPoint.setPosition(position);
expectedYoFrameEuclideanTrajectoryPoint.setLinearVelocity(linearVelocity);
assertEquals(3.4, yoFrameEuclideanTrajectoryPoint.getTime(), 1e-7);
assertEquals(3.4, expectedYoFrameEuclideanTrajectoryPoint.getTime(), 1e-7);
assertTrue(expectedYoFrameEuclideanTrajectoryPoint.epsilonEquals(yoFrameEuclideanTrajectoryPoint, 1e-10));

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

String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameEuclideanTrajectoryPoint testedYoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint(expectedNamePrefix, expectedNameSuffix,
                                                      new YoVariableRegistry("schnoop"),
                                                      expectedFrame);
testedYoFrameEuclideanTrajectoryPoint.set(expectedTime, expectedPosition, expectedLinearVelocity);
testedYoFrameEuclideanTrajectoryPoint.setToZero();
testedYoFrameEuclideanTrajectoryPoint.registerReferenceFrame(expectedFrame);
testedYoFrameEuclideanTrajectoryPoint.switchCurrentReferenceFrame(worldFrame);
testedYoFrameEuclideanTrajectoryPoint.registerReferenceFrame(worldFrame);
testedYoFrameEuclideanTrajectoryPoint.set(expectedTime, expectedPosition, expectedLinearVelocity);
testedYoFrameEuclideanTrajectoryPoint.switchCurrentReferenceFrame(expectedFrame);

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

String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameEuclideanTrajectoryPoint testedYoFrameEuclideanTrajectoryPoint = new YoFrameEuclideanTrajectoryPoint(expectedNamePrefix, expectedNameSuffix,
                                                      new YoVariableRegistry("schnoop"),
                                                      expectedFrame);
testedYoFrameEuclideanTrajectoryPoint.set(expectedTime, expectedPosition, expectedLinearVelocity);
  testedYoFrameEuclideanTrajectoryPoint.registerReferenceFrame(expectedFrame);
  testedYoFrameEuclideanTrajectoryPoint.changeFrame(expectedFrame);

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

public double getLastWaypointTime()
{
 return waypoints.get(numberOfWaypoints.getIntegerValue() - 1).getTime();
}

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

@Override
public void setToNaN(ReferenceFrame referenceFrame)
{
 super.setToNaN(referenceFrame);
 setToNaN();
}

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

public MultipleWaypointsPositionTrajectoryGenerator(String namePrefix, int maximumNumberOfWaypoints, boolean allowMultipleFrames,
   ReferenceFrame referenceFrame, YoVariableRegistry parentRegistry)
{
 super(allowMultipleFrames, referenceFrame);
 this.namePrefix = namePrefix;
 this.maximumNumberOfWaypoints = maximumNumberOfWaypoints;
 registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
 numberOfWaypoints = new IntegerYoVariable(namePrefix + "NumberOfWaypoints", registry);
 numberOfWaypoints.set(0);
 waypoints = new ArrayList<>(maximumNumberOfWaypoints);
 currentTrajectoryTime = new DoubleYoVariable(namePrefix + "CurrentTrajectoryTime", registry);
 currentWaypointIndex = new IntegerYoVariable(namePrefix + "CurrentWaypointIndex", registry);
 subTrajectory = new VelocityConstrainedPositionTrajectoryGenerator(namePrefix + "SubTrajectory", allowMultipleFrames, referenceFrame, registry);
 registerTrajectoryGeneratorsInMultipleFrames(subTrajectory);
 for (int i = 0; i < maximumNumberOfWaypoints; i++)
 {
   YoFrameEuclideanTrajectoryPoint waypoint = new YoFrameEuclideanTrajectoryPoint(namePrefix, "AtWaypoint" + i, registry, referenceFrame);
   waypoints.add(waypoint);
   if (allowMultipleFrames)
    registerMultipleFramesHolders(waypoint);
 }
 clear();
 parentRegistry.addChild(registry);
}

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

@Override
protected void putYoValuesIntoFrameWaypoint()
{
 frameWaypoint.setToZero(getReferenceFrame());
 frameWaypoint.setTime(time.getDoubleValue());
 frameWaypoint.setPosition(position);
 frameWaypoint.setLinearVelocity(linearVelocity);
}

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

@Override
public void initialize()
{
 if (numberOfWaypoints.getIntegerValue() == 0)
 {
   throw new RuntimeException("Trajectory has no waypoints.");
 }
 currentWaypointIndex.set(0);
 if (numberOfWaypoints.getIntegerValue() == 1)
 {
   subTrajectory.setConstant(waypoints.get(0).getPosition());
 }
 else
   initializeSubTrajectory(0);
}

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

@Override
public void setToNaN()
{
 super.setToNaN();
 setTimeToNaN();
 setPositionToNaN();
 setLinearVelocityToNaN();
}

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

public double getLastWaypointTime()
{
 return waypoints.get(numberOfWaypoints.getIntegerValue() - 1).getTime();
}

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

YoFrameEuclideanTrajectoryPoint

相关文章

    查看更多

    热门标签

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

    最新文章

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