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

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

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

YoConcatenatedSplines介绍

暂无

代码示例

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

public void compute(double time)
{
 timeIntoStep.set(time);
 double totalTime = stepTime.getDoubleValue();
 if (time > totalTime)
   time = totalTime;
 concatenatedSplinesWithArcLengthCalculatedIteratively.compute(time);
 concatenatedSplinesWithArcLengthCalculatedIteratively.getPosition(desiredPosition);
 concatenatedSplinesWithArcLengthCalculatedIteratively.getVelocity(desiredVelocity);
 concatenatedSplinesWithArcLengthCalculatedIteratively.getAcceleration(desiredAcceleration);
 // concatenatedSplinesWithArcLengthCalculatedIteratively.compute(time - stepTime.getDoubleValue() / (double) numberOfTimeIntervals);
 // FrameVector previousVelocity = concatenatedSplinesWithArcLengthCalculatedIteratively.getVelocity();
 //
 // concatenatedSplinesWithArcLengthCalculatedIteratively.compute(time + stepTime.getDoubleValue() / (double) numberOfTimeIntervals);
 // FrameVector nextVelocity = concatenatedSplinesWithArcLengthCalculatedIteratively.getVelocity();
 //
 // desiredAcceleration.set(nextVelocity);
 // desiredAcceleration.sub(previousVelocity);
 // desiredAcceleration.scale(0.5 * (double) numberOfTimeIntervals / stepTime.getDoubleValue());
}

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

public double approximateTimeFromArcLength(double arcLength)
{
 if (arcLength > this.arcLength.getDoubleValue())
 {
   return getTf() - getT0();
 }
 int splineIndex = 0;
 double runningArcLengthCounter = arcLength;
 double runningTimeCounter = getT0();
 double deltaArcLength = splines.get(splineIndex).getArcLength();
 while (runningArcLengthCounter - deltaArcLength > EPSILON)
 {
   runningArcLengthCounter -= deltaArcLength;
   runningTimeCounter += splines.get(splineIndex).getTotalTime();
   splineIndex++;
   deltaArcLength = splines.get(splineIndex).getArcLength();
 }
 runningTimeCounter += splines.get(splineIndex).getApproximateTimeForArcLength(runningArcLengthCounter);
 return runningTimeCounter;
}

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

FrameVector3D[] velocities = getRandomVelocities(4);
YoConcatenatedSplines originalSplines = new YoConcatenatedSplines(new int[] {4, 6, 4}, worldFrame, 100, registry, "original");
YoConcatenatedSplines respacedSplines = new YoConcatenatedSplines(new int[] {6, 6, 6, 6}, worldFrame, 100, registry, "respaced");
originalSplines.setCubicQuinticCubic(times, positions, velocities);
respacedSplines.setQuintics(originalSplines, oldTimes, newTimes);
 originalSplines.compute(oldTimes[i]);
 respacedSplines.compute(newTimes[i]);
 FramePoint3D originalPosition = originalSplines.getPosition();
 FramePoint3D respacedPosition = respacedSplines.getPosition();
 FrameVector3D originalVelocity = originalSplines.getVelocity();
 FrameVector3D respacedVelocity = respacedSplines.getVelocity();
 for (Axis axis : Axis.values())

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

@ContinuousIntegrationTest(estimatedDuration = 1.7)
@Test(timeout = 30000)
public void testTimeFromArcLength()
{
 int trials = 50;
 for (int trial = 0; trial < trials; trial++)
 {
   YoVariableRegistry registry = new YoVariableRegistry("ConcatenatedSplinesTest");
   double[] times = getRandomTimes(4);
   FramePoint3D[] positions = getRandomPositions(4);
   FrameVector3D[] velocities = getRandomVelocities(4);
   YoConcatenatedSplines concatenatedSplines = new YoConcatenatedSplines(new int[] {4, 6, 4}, worldFrame, 100, registry, "original");
   concatenatedSplines.setCubicQuinticCubic(times, positions, velocities);
   double arcLengthThroughSecondSpline = concatenatedSplines.getSplineByIndex(0).getArcLength() + concatenatedSplines.getSplineByIndex(1).getArcLength();
   double arcLengthThroughThirdSpline = arcLengthThroughSecondSpline + concatenatedSplines.getSplineByIndex(2).getArcLength();
   double arcLengthSomewhereInBetween = (arcLengthThroughSecondSpline + arcLengthThroughThirdSpline) / 2.0;
   double actualTimeThroughSecondSpline = concatenatedSplines.approximateTimeFromArcLength(arcLengthThroughSecondSpline);
   double actualTimeThroughThirdSpline = concatenatedSplines.approximateTimeFromArcLength(arcLengthThroughThirdSpline);
   double actualTimeInBetween = concatenatedSplines.approximateTimeFromArcLength(arcLengthSomewhereInBetween);
   double expectedTimeThroughSecondSpline = times[2];
   double expectedTimeThroughThirdSpline = times[3];
   assertEquals(expectedTimeThroughSecondSpline, actualTimeThroughSecondSpline, EPSILON);
   assertEquals(expectedTimeThroughThirdSpline, actualTimeThroughThirdSpline, EPSILON);
   assertTrue(expectedTimeThroughSecondSpline <= actualTimeInBetween);
   assertTrue(actualTimeInBetween <= expectedTimeThroughThirdSpline);
 }
}

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

public void setQuintics(double[] times, FramePoint3D[] positions, FrameVector3D[] velocities, FrameVector3D[] accelerations)
{
 MathTools.checkEquals(times.length, rangeList.size() + 1);
 MathTools.checkEquals(positions.length, times.length);
 MathTools.checkEquals(velocities.length, times.length);
 MathTools.checkEquals(accelerations.length, times.length);
 for (int i = 0; i < times.length - 1; i++)
 {
   YoSpline3D spline = splines.get(i);
   double t0 = times[i];
   double tf = times[i + 1];
   FramePoint3D p0 = positions[i];
   FramePoint3D pf = positions[i + 1];
   FrameVector3D pd0 = velocities[i];
   FrameVector3D pdf = velocities[i + 1];
   FrameVector3D pdd0 = accelerations[i];
   FrameVector3D pddf = accelerations[i + 1];
   spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);
   rangeList.get(i).getLeft().set(t0);
   rangeList.get(i).getRight().set(tf);
 }
 setArcLength();
}

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

public void compute(double t)
{
 currentSplineIndex.set(getSplineIndex(t));
 YoSpline3D desiredSpline = splines.get(currentSplineIndex.getIntegerValue());
 desiredSpline.compute(t);
 desiredSpline.getPosition(position);
 desiredSpline.getVelocity(velocity);
 desiredSpline.getAcceleration(acceleration);
}

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

private double[] getArcLengthsCalculatedIteratively()
{
 double[] arcLengths = new double[5];
 for (int i = 0; i < arcLengths.length; i++)
 {
   arcLengths[i] = concatenatedSplinesWithArcLengthApproximatedByDistance.getSplineByIndex(i).getArcLength();
 }
 return arcLengths;
}

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

concatenatedSplinesWithArcLengthApproximatedByDistance = new YoConcatenatedSplines(new int[] {4, 2, 6, 2, 4}, referenceFrame,
   arcLengthCalculatorDivisionsPerPolynomial, registry, namePrefix + "ConcatenatedSplinesWithArcLengthApproximatedByDistance");
concatenatedSplinesWithArcLengthCalculatedIteratively = new YoConcatenatedSplines(new int[] {4, 2, 6, 2, 4}, referenceFrame, 2, registry,
   namePrefix + "ConcatenatedSplinesWithArcLengthCalculatedIteratively");

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

public void setQuintics(double[] times, FramePoint[] positions, FrameVector[] velocities, FrameVector[] accelerations)
{
 MathTools.checkIfEqual(times.length, rangeList.size() + 1);
 MathTools.checkIfEqual(positions.length, times.length);
 MathTools.checkIfEqual(velocities.length, times.length);
 MathTools.checkIfEqual(accelerations.length, times.length);
 for (int i = 0; i < times.length - 1; i++)
 {
   YoSpline3D spline = splines.get(i);
   double t0 = times[i];
   double tf = times[i + 1];
   FramePoint p0 = positions[i];
   FramePoint pf = positions[i + 1];
   FrameVector pd0 = velocities[i];
   FrameVector pdf = velocities[i + 1];
   FrameVector pdd0 = accelerations[i];
   FrameVector pddf = accelerations[i + 1];
   spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);
   rangeList.get(i).getLeft().set(t0);
   rangeList.get(i).getRight().set(tf);
 }
 setArcLength();
}

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

public void compute(double t)
{
 currentSplineIndex.set(getSplineIndex(t));
 YoSpline3D desiredSpline = splines.get(currentSplineIndex.getIntegerValue());
 desiredSpline.compute(t);
 desiredSpline.getPosition(position);
 desiredSpline.getVelocity(velocity);
 desiredSpline.getAcceleration(acceleration);
}

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

public void setQuintics(YoConcatenatedSplines oldSplines, double[] oldTimes, double[] newTimes)
{
 MathTools.checkEquals(oldTimes.length, newTimes.length);
 MathTools.checkEquals(oldTimes.length, rangeList.size() + 1);
 for (int i = 0; i < oldTimes.length - 1; i++)
 {
   YoSpline3D spline = splines.get(i);
   oldSplines.compute(oldTimes[i]);
   double t0 = newTimes[i];
   FramePoint3D p0 = oldSplines.getPosition();
   FrameVector3D pd0 = oldSplines.getVelocity();
   FrameVector3D pdd0 = oldSplines.getAcceleration();
   oldSplines.compute(oldTimes[i + 1]);
   double tf = newTimes[i + 1];
   FramePoint3D pf = oldSplines.getPosition();
   FrameVector3D pdf = oldSplines.getVelocity();
   FrameVector3D pddf = oldSplines.getAcceleration();
   spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);
   rangeList.get(i).getLeft().set(t0);
   rangeList.get(i).getRight().set(tf);
 }
 setArcLength();
}

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

@ContinuousIntegrationTest(estimatedDuration = 1.7)
@Test(timeout = 30000)
public void testSimpleCubicQuinticCubic()
{
 int trials = 50;
 for (int trial = 0; trial < trials; trial++)
 {
   YoVariableRegistry registry = new YoVariableRegistry("ConcatenatedSplinesTest");
   double[] times = getRandomTimes(4);
   FramePoint3D[] positions = getRandomPositions(4);
   FrameVector3D[] velocities = getRandomVelocities(4);
      YoConcatenatedSplines concatenatedSplines = new YoConcatenatedSplines(new int[] {4, 6, 4}, worldFrame, 100, registry, "original");
   concatenatedSplines.setCubicQuinticCubic(times, positions, velocities);
   for (int i = 0; i < 4; i++)
   {
    concatenatedSplines.compute(times[i]);
    FramePoint3D expectedPosition = positions[i];
    FramePoint3D actualPosition = concatenatedSplines.getPosition();
    FrameVector3D expectedVelocity = velocities[i];
    FrameVector3D actualVelocity = concatenatedSplines.getVelocity();
    for (Axis axis : Axis.values())
    {
      assertEquals(expectedPosition.getElement(axis.ordinal()), actualPosition.getElement(axis.ordinal()), EPSILON);
      assertEquals(expectedVelocity.getElement(axis.ordinal()), actualVelocity.getElement(axis.ordinal()), EPSILON);
    }
   }
 }
}

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

public double approximateTimeFromArcLength(double arcLength)
{
 if (arcLength > this.arcLength.getDoubleValue())
 {
   return getTf() - getT0();
 }
 int splineIndex = 0;
 double runningArcLengthCounter = arcLength;
 double runningTimeCounter = getT0();
 double deltaArcLength = splines.get(splineIndex).getArcLength();
 while (runningArcLengthCounter - deltaArcLength > EPSILON)
 {
   runningArcLengthCounter -= deltaArcLength;
   runningTimeCounter += splines.get(splineIndex).getTotalTime();
   splineIndex++;
   deltaArcLength = splines.get(splineIndex).getArcLength();
 }
 runningTimeCounter += splines.get(splineIndex).getApproximateTimeForArcLength(runningArcLengthCounter);
 return runningTimeCounter;
}

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

setArcLength();

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

public void setQuintics(YoConcatenatedSplines oldSplines, double[] oldTimes, double[] newTimes)
{
 MathTools.checkIfEqual(oldTimes.length, newTimes.length);
 MathTools.checkIfEqual(oldTimes.length, rangeList.size() + 1);
 for (int i = 0; i < oldTimes.length - 1; i++)
 {
   YoSpline3D spline = splines.get(i);
   oldSplines.compute(oldTimes[i]);
   double t0 = newTimes[i];
   FramePoint p0 = oldSplines.getPosition();
   FrameVector pd0 = oldSplines.getVelocity();
   FrameVector pdd0 = oldSplines.getAcceleration();
   oldSplines.compute(oldTimes[i + 1]);
   double tf = newTimes[i + 1];
   FramePoint pf = oldSplines.getPosition();
   FrameVector pdf = oldSplines.getVelocity();
   FrameVector pddf = oldSplines.getAcceleration();
   spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);
   rangeList.get(i).getLeft().set(t0);
   rangeList.get(i).getRight().set(tf);
 }
 setArcLength();
}

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

FrameVector3D[] velocities = new FrameVector3D[]{v0, v1, v2, v3};
YoConcatenatedSplines concatenatedSplines = new YoConcatenatedSplines(new int[] {4, 6, 4}, worldFrame, 100, registry, "original");
concatenatedSplines.setCubicQuinticCubic(times, positions, velocities);
  concatenatedSplines.compute(times[i]);
  FramePoint3D expectedPosition = positions[i];
  FramePoint3D actualPosition = concatenatedSplines.getPosition();
  FrameVector3D expectedVelocity = velocities[i];
  FrameVector3D actualVelocity = concatenatedSplines.getVelocity();
  for (Axis axis : Axis.values())

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

private void visualizeSpline()
{
 double t0;
 double tf;
 double t;
 for (int i = 0; i < numberOfVisualizationMarkers; i++)
 {
   t0 = concatenatedSplinesWithArcLengthCalculatedIteratively.getT0();
   tf = concatenatedSplinesWithArcLengthCalculatedIteratively.getTf();
   t = t0 + (double) i / (double) (numberOfVisualizationMarkers) * (tf - t0);
   compute(t);
   trajectoryBagOfBalls.setBall(desiredPosition.getFramePointCopy(), i);
 }
 for (int i = 0; i < nonAccelerationEndpointIndices.length; i++)
 {
   fixedPointBagOfBalls.setBall(allPositions[nonAccelerationEndpointIndices[i]].getFramePointCopy(), YoAppearance.AliceBlue(), i);
 }
}

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

setArcLength();

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

setArcLength();

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

setArcLength();

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

YoConcatenatedSplines

相关文章

    查看更多

    热门标签

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

    最新文章

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