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

YoSpline3D介绍

暂无

代码示例

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

private YoSpline3D generateRandomQuintic(Random random)
{
 YoVariableRegistry registry = new YoVariableRegistry("Spline3DTest");
  YoSpline3D spline = new YoSpline3D(6, 4, worldFrame, registry, "");
    double t0 = random.nextDouble() - 1;
  double tf = random.nextDouble();
  FramePoint3D p0 = new FramePoint3D(worldFrame, 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1));
  FrameVector3D pd0 = new FrameVector3D(worldFrame, 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1));
  FrameVector3D pdd0 = new FrameVector3D(worldFrame, 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1));   
  FramePoint3D pf = new FramePoint3D(worldFrame, 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1));
  FrameVector3D pdf = new FrameVector3D(worldFrame, 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1));
  FrameVector3D pddf = new FrameVector3D(worldFrame, 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1), 2 * (random.nextDouble() - 1)); 	   
  spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);
    assertEquals(t0, spline.getT0(), 1e-7);
  assertEquals(tf, spline.getTf(), 1e-7);
  assertEquals(Math.abs(tf - t0), spline.getTotalTime(), 1e-7);
  assertEquals(spline.getArcLength(), spline.getArcLength(t0, tf), 1e-7);
  return spline;
}

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

spline.setCubic(t0, tf, p0, pd0, pf, pdf);
splines.get(0).compute(t0);
FrameVector3D pdd0 = splines.get(0).getAccelerationCopy();
spline.setQuarticUsingInitialAcceleration(t0, tf, p0, pd0, pdd0, pf, pdf);
splines.get(4).compute(tf);
FrameVector3D pddf = splines.get(4).getAccelerationCopy();
spline.setQuarticUsingFinalAcceleration(t0, tf, p0, pd0, pf, pdf, pddf);
splines.get(1).compute(t0);
FrameVector3D pdd0 = splines.get(1).getAccelerationCopy();
splines.get(3).compute(tf);
FrameVector3D pddf = splines.get(3).getAccelerationCopy();
spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);

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

spline.setQuadraticUsingInitialVelocity(t0, tf, p0, pd0, pf);
spline.setQuadraticUsingFinalVelocity(t0, tf, p0, pf, pdf);
splines.get(0).compute(times[i]);
FrameVector pd0 = splines.get(0).getVelocityCopy();
FrameVector pdd0 = splines.get(0).getAccelerationCopy();
splines.get(2).compute(times[i + 1]);
FrameVector pdf = splines.get(2).getVelocityCopy();
FrameVector pddf = splines.get(2).getAccelerationCopy();
spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);
splines.get(1).compute(splines.get(1).getT0());

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

spline.setLinearUsingFinalPositionAndVelocity(t0, tf, pf, pdf);
spline.setLinearUsingInitialPositionAndVelocity(t0, tf, p0, pd0);
splines.get(1).compute(tf);
pf = splines.get(1).getPositionCopy();
FrameVector pddf = zero;
spline.setQuarticUsingFinalAcceleration(t0, tf, p0, pd0, pf, pdf, pddf);
splines.get(3).compute(t0);
p0 = splines.get(3).getPositionCopy();
FrameVector pdd0 = zero;
spline.setQuarticUsingInitialAcceleration(t0, tf, p0, pd0, pdd0, pf, pdf);
splines.get(1).compute(t0);
FrameVector pdd0 = zero;
splines.get(3).compute(tf);
FrameVector pddf = zero;
spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);

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

spline.setLinearUsingFinalPositionAndVelocity(t0, tf, pf, pdf);
spline.setLinearUsingInitialPositionAndVelocity(t0, tf, p0, pd0);
splines.get(1).compute(tf);
pf = splines.get(1).getPositionCopy();
spline.setCubic(t0, tf, p0, pd0, pf, pdf);
splines.get(3).compute(t0);
p0 = splines.get(3).getPositionCopy();
spline.setCubic(t0, tf, p0, pd0, pf, pdf);
splines.get(1).compute(t0);
FrameVector pdd0 = zero;
splines.get(3).compute(tf);
FrameVector pddf = zero;
spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);

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

spline.setSexticUsingWaypointVelocityAndAccelerationAndFinalAcceleration(t0, t1, tf, p0, pd0, pd1, pdd1, pf, pdf, pddf);
spline.setSexticUsingWaypointVelocityAndAccelerationAndInitialAcceleration(t0, t1, tf, p0, pd0, pdd0, pd1, pdd1, pf, pdf);
splines.get(0).compute(t0);
FrameVector pdd0 = splines.get(0).getAccelerationCopy();
splines.get(2).compute(tf);
FrameVector pddf = splines.get(2).getAccelerationCopy();
spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testArcLengthMethods()
{
  // find arc length from t=0 to t=1 of: x = t, y = t, z = t
 YoVariableRegistry registry = new YoVariableRegistry("Spline3DTest");
  YoSpline3D spline = new YoSpline3D(4, 500, worldFrame, registry, "");
    double t0 = 0.0;
  double tf = 1.0;
    FramePoint3D p0 = new FramePoint3D(worldFrame, 0.0, 0.0, 0.0);
  FrameVector3D pd0 = new FrameVector3D(worldFrame, 1.0, 1.0, 1.0);
  FramePoint3D pf = new FramePoint3D(worldFrame, 1.0, 1.0, 1.0);
  FrameVector3D pdf = new FrameVector3D(worldFrame, 1.0, 1.0, 1.0);
    spline.setCubic(t0, tf, p0, pd0, pf, pdf);
    double actual = spline.getArcLength(0.0, 1.0);
  assertEquals(actual, Math.sqrt(3.0), 1e-7);
    Random random = new Random(46522L);
  for(int i = 0; i < 50; i++)
  {
    double randomDouble = random.nextDouble();
    assertEquals(spline.getApproximateTimeForArcLength(randomDouble * Math.sqrt(3.0)), randomDouble, 1e-7);		   
  }
}

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

spline.setQuarticUsingIntermediateVelocity(t0, t1, tf, p0, pd0, pd1, pf, pdf);
splines.get(0).compute(t0);
FrameVector3D pdd0 = splines.get(0).getAccelerationCopy();
splines.get(2).compute(tf);
FrameVector3D pddf = splines.get(2).getAccelerationCopy();
spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);

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

FrameVector pd1 = intermediateVelocities[i / 2];
FrameVector pdd1 = intermediateAccelerations[i / 2];
spline.setQuinticUsingIntermediateVelocityAndAcceleration(t0, t1, tf, p0, pd0, pd1, pdd1, pf, pdf);
splines.get(0).compute(t0);
FrameVector pdd0 = splines.get(0).getAccelerationCopy();
splines.get(2).compute(tf);
FrameVector pddf = splines.get(2).getAccelerationCopy();
spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testCubic()
{
 //cubic is constructed such that x(t) = t^3 + 2t^2 + t + 1, y(t) = 2t^3 + t + 4, z(t) = t^2 + 7
 YoVariableRegistry registry = new YoVariableRegistry("Spline3DTest");
 YoSpline3D cubic = new YoSpline3D(4, 4, worldFrame, registry, "");
 double t0 = 1.0;
 double tf = 5.0;
 FramePoint3D p0 = new FramePoint3D(worldFrame, 5.0, 7.0, 8.0);
 FrameVector3D pd0 = new FrameVector3D(worldFrame, 8.0, 7.0, 2.0);
 FramePoint3D pf = new FramePoint3D(worldFrame, 181.0, 259.0, 32.0);
 FrameVector3D pdf = new FrameVector3D(worldFrame, 96.0, 151.0, 10.0);
 
 cubic.setCubic(t0, tf, p0, pd0, pf, pdf);
 
 FramePoint3D expected = new FramePoint3D(worldFrame, 49.0, 61.0, 16.0);
 cubic.compute(3.0);
 FramePoint3D actual = cubic.getPositionCopy();
 
 for (Axis axis : Axis.values())
 {
   assertEquals(expected.getElement(axis.ordinal()), actual.getElement(axis.ordinal()), EPSILON);
 }
 
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testQuintic()
{
 YoVariableRegistry registry = new YoVariableRegistry("Spline3DTest");
  YoSpline3D quintic = new YoSpline3D(6, 4, worldFrame, registry, "");
  // quintic is constructed such that x(t) = t^5 - t^3 + t - 1, y(t) = 2t^5 - 4, z(t) = t^5 + 2t^4
  double t0 = 0.0;
  double tf = 1.0;
  FramePoint3D p0 = new FramePoint3D(worldFrame, -1.0, -4.0, 0.0);
  FrameVector3D pd0 = new FrameVector3D(worldFrame, 1.0, 0.0, 0.0);
  FrameVector3D pdd0 = new FrameVector3D(worldFrame, 0.0, 0.0, 0.0);   
  FramePoint3D pf = new FramePoint3D(worldFrame, 0.0, -2.0, 3.0);
  FrameVector3D pdf = new FrameVector3D(worldFrame, 3.0, 10.0, 13.0);
  FrameVector3D pddf = new FrameVector3D(worldFrame, 14.0, 40.0, 44.0);
  quintic.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf);
    FramePoint3D expected = new FramePoint3D(worldFrame, -0.59375, -3.9375, 0.15625);
  quintic.compute(0.5);
  FramePoint3D actual = quintic.getPositionCopy();
   
  for (Axis axis : Axis.values())
  {
   assertEquals(expected.getElement(axis.ordinal()), actual.getElement(axis.ordinal()), EPSILON);
  }   
}

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

FrameVector3D pd1 = intermediateVelocities[i / 2];
FrameVector3D pdd1 = intermediateAccelerations[i / 2];
spline.setQuinticUsingIntermediateVelocityAndAcceleration(t0, t1, tf, p0, pd0, pd1, pdd1, pf, pdf);
splines.get(0).compute(t0);
FrameVector3D pdd0 = splines.get(0).getAccelerationCopy();
splines.get(2).compute(tf);
FrameVector3D pddf = splines.get(2).getAccelerationCopy();
spline.setSexticUsingWaypoint(t0, ghostTime, tf, p0, pd0, pdd0, ghostWaypoint, pf, pdf, pddf);

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

private void setArcLengths()
{
 pi.setToZero(referenceFrame);
 piPlusOne.setToZero(referenceFrame);
 
 arcLengths[0].set(0.0);
 double tiPlusOne = t0.getDoubleValue();
 compute(t0.getDoubleValue());
 getPosition(piPlusOne);
 double differentialDistance;
 for (int i = 0; i < arcLengthCalculatorDivisions; i++)
 {
   pi.set(piPlusOne);
   tiPlusOne = getTime(i + 1);
   compute(tiPlusOne);
   getPosition(piPlusOne);
   differentialDistance = pi.distance(piPlusOne);
   arcLengths[i + 1].set(arcLengths[i].getDoubleValue() + differentialDistance);
 }
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testGetVelocity()
{
  Random random = new Random(557975L);
  YoSpline3D spline = generateRandomQuintic(random);
    double t = 2 * (random.nextDouble() - 1);
  double dt = 1e-9;
  spline.compute(t);
  FrameVector3D actual = spline.getVelocityCopy();
  FramePoint3D pos1 = spline.getPositionCopy();
  spline.compute(t + dt);
  FramePoint3D pos2 = spline.getPositionCopy();
  FrameVector3D numerical = new FrameVector3D(worldFrame, (pos2.getX() - pos1.getX()) / dt, (pos2.getY() - pos1.getY()) / dt, (pos2.getZ() - pos1.getZ()) / dt);
  assertEquals(numerical.getX(), actual.getX(), 1e-5);
  assertEquals(numerical.getY(), actual.getY(), 1e-5);
  assertEquals(numerical.getZ(), actual.getZ(), 1e-5);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testGetAcceleration()
{
  Random random = new Random(4561955L);
  YoSpline3D spline = generateRandomQuintic(random);
  double t = 2 * (random.nextDouble() - 1);
  double dt = 1e-9;
  spline.compute(t);
  FrameVector3D actual = spline.getAccelerationCopy();
  FrameVector3D vel1 = spline.getVelocityCopy();
  spline.compute(t + dt);
  FrameVector3D vel2 = spline.getVelocityCopy();
  FrameVector3D numerical = new FrameVector3D(worldFrame, (vel2.getX() - vel1.getX()) / dt, (vel2.getY() - vel1.getY()) / dt, (vel2.getZ() - vel1.getZ()) / dt);
  assertEquals(numerical.getX(), actual.getX(), Math.abs(actual.getX()) * 1e-5);
  assertEquals(numerical.getY(), actual.getY(), Math.abs(actual.getY()) * 1e-5);
  assertEquals(numerical.getZ(), actual.getZ(), Math.abs(actual.getZ()) * 1e-5);
}

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

public double getApproximateTimeForArcLength(double arcLength)
 if (arcLength > getArcLength())
 double upperBound = arcLengths[upperBoundIndex].getDoubleValue();
 double proportionBetweenBounds = (arcLength - lowerBound) / (upperBound - lowerBound);
 double lowerBoundTime = getTime(lowerBoundIndex);
 double upperBoundTime = getTime(upperBoundIndex);

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

private void setArcLength()
{
 double arcLength = 0.0;
 for (YoSpline3D spline : splines)
 {
   arcLength += spline.getArcLength();
 }
 this.arcLength.set(arcLength);
}

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

public YoConcatenatedSplines(int[] numberOfCoefficientsPerPolynomial, ReferenceFrame referenceFrame, int arcLengthCalculatorDivisionsPerPolynomial,
              YoVariableRegistry parentRegistry, String namePrefix)
{
 registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
 parentRegistry.addChild(registry);
 this.referenceFrame = referenceFrame;
 this.splines = new ArrayList<YoSpline3D>();
 rangeList = new ArrayList<ImmutablePair<DoubleYoVariable, DoubleYoVariable>>();
 for (int i = 0; i < numberOfCoefficientsPerPolynomial.length; i++)
 {
   splines.add(new YoSpline3D(numberOfCoefficientsPerPolynomial[i], arcLengthCalculatorDivisionsPerPolynomial, referenceFrame, registry,
                namePrefix + "Spline" + i));
   rangeList.add(new ImmutablePair<DoubleYoVariable, DoubleYoVariable>(new DoubleYoVariable(namePrefix + "Range" + i + "First", registry),
              new DoubleYoVariable(namePrefix + "Range" + i + "Second", registry)));
 }
 position = new YoFramePoint(namePrefix + "Position", referenceFrame, registry);
 velocity = new YoFrameVector(namePrefix + "Velocity", referenceFrame, registry);
 acceleration = new YoFrameVector(namePrefix + "Acceleration", referenceFrame, registry);
 arcLength = new DoubleYoVariable(namePrefix + "ArcLength", registry);
 
 currentSplineIndex = new IntegerYoVariable(namePrefix + "CurrentSplineIndex", registry);
}