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

YoTrajectory介绍

暂无

代码示例

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testGetDerivative()
  {
   YoTrajectory traj = new YoTrajectory("Trajectory", 10, registry);
   YoTrajectory dervTraj = new YoTrajectory("DerivativeTrajectory", 9, registry);
   traj.setCubic(1, 10, 0, 8);
   traj.getDerivative(dervTraj, 1);
      assert (dervTraj.getNumberOfCoefficients() == 3);
   assert (MathTools.epsilonCompare(1.0 * traj.getCoefficient(1), dervTraj.getCoefficient(0), 1));
   assert (MathTools.epsilonCompare(2.0 * traj.getCoefficient(2), dervTraj.getCoefficient(1), 1));
   assert (MathTools.epsilonCompare(3.0 * traj.getCoefficient(3), dervTraj.getCoefficient(2), 1));      
  }
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

public double calculateICPLinearManual(YoTrajectory linear, double icpPositionDesiredFinal, double t, double omega0)
{
 double icpManual = 0.0;
 
 linear.compute(linear.getInitialTime());
 double cmpRefInit = linear.getPosition();
 
 linear.compute(linear.getFinalTime());
 double cmpRefFinal = linear.getPosition();
 
 double T = linear.getFinalTime();
 
 double sigmat = calculateSigmaLinear(t, T, omega0);
 double sigmaT = calculateSigmaLinear(T, T, omega0);
 
 icpManual = (1 - sigmat - Math.exp(omega0*(t-T)) * (1 - sigmaT)) * cmpRefInit
       + (sigmat - Math.exp(omega0*(t-T)) * sigmaT) * cmpRefFinal
       + Math.exp(omega0*(t-T)) * icpPositionDesiredFinal;
 
 return icpManual;
}

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

public void getDerivative(YoTrajectory dervTraj, int order)
{
 if (dervTraj.getMaximumNumberOfCoefficients() < this.getNumberOfCoefficients() - order)
   return;
 dervTraj.polynomial.reshape(getNumberOfCoefficients() - order);
 for (int i = order; i < this.getNumberOfCoefficients(); i++)
 {
   dervTraj.polynomial.setDirectlyFast(i - order, this.polynomial.getDerivativeCoefficient(order, i) * this.polynomial.getCoefficient(i));
 }
 dervTraj.setInitialTime(this.getInitialTime());
 dervTraj.setFinalTime(this.getFinalTime());
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

YoTrajectory linear = new YoTrajectory(namePrefix + "Linear", numberOfCoefficients, registry);
  double y0 = Math.signum(Math.random()) * Math.random() * scaleY0, yf = Math.signum(Math.random()) * Math.random() * scaleYf;
  linear.setLinear(x0, xf, y0, yf);
  DenseMatrix64F polynomialCoefficientVector =  linear.getCoefficientsVector();
  linear.compute(linear.getFinalTime());
  double icpPositionDesiredFinal = linear.getPosition();

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

private void calculateGeneralizedBetaPrimeOnCMPSegment(DenseMatrix64F generalizedBetaPrime, int betaDerivativeOrder, YoTrajectory cmpPolynomial, double time, double omega0)
{            
 int numberOfCoefficients = cmpPolynomial.getNumberOfCoefficients();
 double timeSegmentTotal = cmpPolynomial.getFinalTime() - cmpPolynomial.getInitialTime();
 
 DenseMatrix64F tPowersDerivativeVector = new DenseMatrix64F(numberOfCoefficients, 1);
 DenseMatrix64F tPowersDerivativeVectorTranspose = new DenseMatrix64F(1, numberOfCoefficients);
 for(int i = 0; i < numberOfCoefficients; i++)
 {
   tPowersDerivativeVector.zero();
   tPowersDerivativeVectorTranspose.zero();
   tPowersDerivativeVector.set(cmpPolynomial.getXPowersDerivativeVector(i, timeSegmentTotal));
   CommonOps.transpose(tPowersDerivativeVector, tPowersDerivativeVectorTranspose);
       
   double scalar = Math.pow(omega0, betaDerivativeOrder-i) * Math.exp(omega0*(time-timeSegmentTotal));
   CommonOps.addEquals(generalizedBetaPrime, scalar, tPowersDerivativeVectorTranspose);
 }
}

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

public boolean timeIntervalContains(double timeToCheck, double EPSILON)
{
 return MathTools.intervalContains(timeToCheck, getInitialTime(), getFinalTime(), EPSILON);
}

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

public boolean isValidTrajectory()
{
 boolean retVal = (getInitialTime() < getFinalTime()) && Double.isFinite(getInitialTime()) && Double.isFinite(getFinalTime());
 double[] coeffs = getCoefficients();
 for(int i = 0; retVal && i < coeffs.length; i++)
   retVal &= Double.isFinite(coeffs[i]);
 return retVal;
}

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

/**
* Returns the number of coefficients for the trajectory if it is the same for all axes. If not then returns -1
* @return
*/
public int getNumberOfCoefficients()
{
 if (xTrajectory.getNumberOfCoefficients() == yTrajectory.getNumberOfCoefficients()
    && xTrajectory.getNumberOfCoefficients() == zTrajectory.getNumberOfCoefficients())
   return xTrajectory.getNumberOfCoefficients();
 else
   return -1;
}

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

YoTrajectory linear = new YoTrajectory(namePrefix + "Linear", numberOfCoefficients, registry);
  double y0 = Math.signum(Math.random()) * Math.random() * scaleY0, yf = Math.signum(Math.random()) * Math.random() * scaleYf;
  linear.setLinear(x0, xf, y0, yf);

代码示例来源：origin: us.ihmc/ihmc-common-walking-control-modules-test

private void calculateGeneralizedAlphaPrimeOnCMPSegment(DenseMatrix64F generalizedAlphaPrime, int alphaDerivativeOrder, YoTrajectory cmpPolynomial, double time, double omega0)
{
 int numberOfCoefficients = cmpPolynomial.getNumberOfCoefficients();
 
 DenseMatrix64F tPowersDerivativeVector = new DenseMatrix64F(numberOfCoefficients, 1);
 DenseMatrix64F tPowersDerivativeVectorTranspose = new DenseMatrix64F(1, numberOfCoefficients);
 
 for(int i = 0; i < numberOfCoefficients; i++)
 {
   tPowersDerivativeVector.zero();
   tPowersDerivativeVectorTranspose.zero();
      tPowersDerivativeVector.set(cmpPolynomial.getXPowersDerivativeVector(i + alphaDerivativeOrder, time));
   CommonOps.transpose(tPowersDerivativeVector, tPowersDerivativeVectorTranspose);
       
   double scalar = Math.pow(omega0, -i);
   CommonOps.addEquals(generalizedAlphaPrime, scalar, tPowersDerivativeVectorTranspose);
 }
}

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

public void set(Trajectory trajectory)
  {
   reshape(trajectory.getNumberOfCoefficients());
   this.tInitial.set(trajectory.getInitialTime());
   this.tFinal.set(trajectory.getFinalTime());
   for(int i = 0; i < getNumberOfCoefficients(); i++)
     polynomial.setDirectly(i, trajectory.getCoefficient(i));
  }
}

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

public YoTrajectory3D(String name, int maximumNumberOfCoefficients, YoVariableRegistry registry)
{
 xTrajectory = new YoTrajectory(name + "X", maximumNumberOfCoefficients, registry);
 yTrajectory = new YoTrajectory(name + "Y", maximumNumberOfCoefficients, registry);
 zTrajectory = new YoTrajectory(name + "Z", maximumNumberOfCoefficients, registry);
}

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

public double getInitialTime()
{
 if (MathTools.epsilonCompare(xTrajectory.getInitialTime(), yTrajectory.getInitialTime(), Epsilons.ONE_THOUSANDTH)
    && MathTools.epsilonCompare(xTrajectory.getInitialTime(), zTrajectory.getInitialTime(), Epsilons.ONE_THOUSANDTH))
   return xTrajectory.getInitialTime();
 else
 {
   //PrintTools.warn("Trajectory initial times do not match. Using X trajectory times for computation");
   return xTrajectory.getInitialTime();
 }
}

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

public double getFinalTime()
{
 if (MathTools.epsilonCompare(xTrajectory.getFinalTime(), yTrajectory.getFinalTime(), Epsilons.ONE_THOUSANDTH)
    && MathTools.epsilonCompare(xTrajectory.getFinalTime(), zTrajectory.getFinalTime(), Epsilons.ONE_THOUSANDTH))
   return xTrajectory.getFinalTime();
 else
 {
   //PrintTools.warn("Trajectory final times do not match. Using X trajectory times for computation");
   return xTrajectory.getFinalTime();
 }
}

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

@Override
public double getX()
{
  return xTrajectory.getPosition();
}

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

public void getDerivative(int order, double x, Tuple3DBasics dTrajectory)
{
 dTrajectory.set(xTrajectory.getDerivative(order, x), yTrajectory.getDerivative(order, x), zTrajectory.getDerivative(order, x));
}

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

public void setCubic(double t0, double tFinal, Point3DReadOnly z0, Point3DReadOnly zFinal)
{
 for (int index = 0; index < 3; index++)
   getYoTrajectory(index).setCubic(t0, tFinal, z0.getElement(index), zFinal.getElement(index));
}

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

public void compute(double t)
{
 xTrajectory.compute(t);
 yTrajectory.compute(t);
 zTrajectory.compute(t);
}

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

public void setLinear(double t0, double tFinal, Point3DReadOnly z0, Point3DReadOnly zf)
{
 for (int index = 0; index < 3; index++)
   getYoTrajectory(index).setLinear(t0, tFinal, z0.getElement(index), zf.getElement(index));
}

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

public boolean timeIntervalContains(double timeToCheck)
{
 return MathTools.intervalContains(timeToCheck, getInitialTime(), getFinalTime(), Epsilons.ONE_MILLIONTH);
}