us.ihmc.robotics.kinematics.fourbar.YoVariableSideFourbarCalculatorWithDerivatives类的使用及代码示例

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

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

YoVariableSideFourbarCalculatorWithDerivatives介绍

暂无

代码示例

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testParallelogram()
{
 YoVariableSideFourbarCalculatorWithDerivatives calculator = new YoVariableSideFourbarCalculatorWithDerivatives("testCalculator", registry);
 calculator.setSideLengths(1.0, 1.0, 1.0, 1.0);
 calculator.updateAnglesAndVelocitiesGivenAngleDAB(PI / 3, 1);
 assertEquals(PI / 3, calculator.getAngleDAB(), eps);
 assertEquals(2 * PI / 3, calculator.getAngleABC(), eps);
 assertEquals(PI / 3, calculator.getAngleBCD(), eps);
 assertEquals(2 * PI / 3, calculator.getAngleCDA(), eps);
 assertEquals(1, calculator.getAngleDtDAB(), eps);
 assertEquals(1, calculator.getAngleDtBCD(), eps);
 assertEquals(-1, calculator.getAngleDtABC(), eps);
 assertEquals(-1, calculator.getAngleDtCDA(), eps);
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSquare()
{
 YoVariableSideFourbarCalculatorWithDerivatives calculator = new YoVariableSideFourbarCalculatorWithDerivatives("testCalculator", registry);
 calculator.setSideLengths(1.0, 1.0, 1.0, 1.0);
 calculator.updateAnglesGivenAngleDAB(PI / 2);
 assertEquals(PI / 2, calculator.getAngleDAB(), eps);
 assertEquals(PI / 2, calculator.getAngleABC(), eps);
 assertEquals(PI / 2, calculator.getAngleBCD(), eps);
 assertEquals(PI / 2, calculator.getAngleCDA(), eps);
}

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

private static double getAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite)
{
 double angle = Math.acos(getCosineAngleWithCosineLaw(l_neighbour1, l_neighbour2, l_opposite));
 return angle;
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSquareDer()
{
 YoVariableSideFourbarCalculatorWithDerivatives calculator = new YoVariableSideFourbarCalculatorWithDerivatives("testCalculator", registry);
 calculator.setSideLengths(1.0, 1.0, 1.0, 1.0);
 calculator.updateAnglesAndVelocitiesGivenAngleDAB(PI / 2.0, 1.0);
 assertEquals(1, calculator.getAngleDtDAB(), eps);
 assertEquals(1, calculator.getAngleDtBCD(), eps);
 assertEquals(-1, calculator.getAngleDtABC(), eps);
 assertEquals(-1, calculator.getAngleDtCDA(), eps);
}

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

private void solveFourBar(YoVariableSideFourbarCalculatorWithDerivatives calculator, double lengthA, double lengthB, double lengthC, double lengthD,
   double A, double B, double C, double D)
{
 if (PRINT)
 {
   String[] names = "a,b,c,d,A,B,C,D".split(",");
   double[] vals = new double[] {lengthA, lengthB, lengthC, lengthD, A, B, C, D};
   for (int i = 0; i < vals.length; i++)
   {
    System.out.println(names[i] + " = " + vals[i]);
   }
 }
 calculator.setSideLengths(lengthA, lengthB, lengthC, lengthD);
 calculator.updateAnglesAndVelocitiesGivenAngleDAB(A, 0.0);
 assertEquals(A, calculator.getAngleDAB(), eps);
 assertEquals(B, calculator.getAngleABC(), eps);
 assertEquals(C, calculator.getAngleBCD(), eps);
 assertEquals(D, calculator.getAngleCDA(), eps);
}

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

public void testMasterAngleComputations()
 YoVariableSideFourbarCalculatorWithDerivatives calculator = new YoVariableSideFourbarCalculatorWithDerivatives("testCalculator", registry);
   double BAD = DAE + BAE, ABC = ABE + CBF, BCD = BCF + DCF, ADC = ADE + CDF;
   calculator.setSideLengths(AD, AB, BC, CD);
   calculator.computeMasterJointAngleGivenAngleABC(ABC);
   assertEquals(BAD, calculator.getAngleDAB(), eps);
   calculator.computeMasterJointAngleGivenAngleBCD(BCD);
   assertEquals(BAD, calculator.getAngleDAB(), eps);
   calculator.computeMasterJointAngleGivenAngleCDA(ADC);
   assertEquals(BAD, calculator.getAngleDAB(), eps);

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

public void setSideLengths(double lengthAD, double lengthBA, double lengthCB, double lengthDC)
{
 this.lengthAD.set(lengthAD);
 this.lengthBA.set(lengthBA);
 this.lengthCB.set(lengthCB);
 this.lengthDC.set(lengthDC);
 double eMax = min(lengthAD + lengthBA, lengthDC + lengthCB);
 if (eMax == lengthAD + lengthBA)
   maxA.set(PI);
 else
   maxA.set(getAngleWithCosineLaw(lengthAD, lengthBA, eMax));
 double fMax = min(lengthAD + lengthDC, lengthBA + lengthCB);
 if (fMax == lengthAD + lengthDC)
   minA.set(getAngleWithCosineLaw(fMax, lengthBA, lengthCB));
 else
   minA.set(getAngleWithCosineLaw(fMax, lengthAD, lengthDC));
}

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

public boolean updateAnglesVelocitiesAndAccelerationsGivenAngleDAB(double angleDABInRadians, double angularVelocityDAB, double angularAccelerationDAB)
{
 // Solve angles and angular velocity
 boolean isAHittingBounds = updateAnglesAndVelocitiesGivenAngleDAB(angleDABInRadians, angularVelocityDAB);
 // Solve angular acceleration
 double A = clamp(angleDABInRadians, minA.getDoubleValue(), maxA.getDoubleValue());
 double dAdT = angularVelocityDAB;
 double dAdT2 = angularAccelerationDAB;
 double e = unknownTriangleSideLengthByLawOfCosine(lengthAD.getDoubleValue(), lengthBA.getDoubleValue(), A);
 double eDot = lengthAD.getDoubleValue() * lengthBA.getDoubleValue() * sin(A) * dAdT / e;
 double eDDot = lengthAD.getDoubleValue() * lengthBA.getDoubleValue() / e * (cos(A) * dAdT * dAdT + sin(A) * (dAdT2 - eDot * dAdT / e));
 double dCdT2 = FourbarCalculatorTools.getAngleDDotWithCosineLaw(lengthCB.getDoubleValue(), lengthDC.getDoubleValue(), 0.0, 0.0, e, eDot, eDDot);
 double angleDDotDBA = FourbarCalculatorTools.getAngleDDotWithCosineLaw(lengthBA.getDoubleValue(), e, eDot, eDDot, lengthAD.getDoubleValue(), 0.0, 0.0);
 double angleDDotDBC = FourbarCalculatorTools.getAngleDDotWithCosineLaw(lengthCB.getDoubleValue(), e, eDot, eDDot, lengthDC.getDoubleValue(), 0.0, 0.0);
 double dBdT2 = angleDDotDBA + angleDDotDBC;
 double dDdT2 = -dAdT2 - dBdT2 - dCdT2;
 this.angleDt2DAB = dAdT2;
 this.angleDt2ABC = dBdT2;
 this.angleDt2BCD = dCdT2;
 this.angleDt2CDA = dDdT2;
 return isAHittingBounds;
}

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

/**
* Compute every angle of the quadrilateral.
* @param angleDABInRadians is the angle formed by the sides lengthAD and lengthBA (see scheme in this class)
* @param angularVelocityDAB first time-derivative of the angle DAB
* @return true if the angle DAB is out of range making the quadrilateral non-convex
*/
public boolean updateAnglesAndVelocitiesGivenAngleDAB(double angleDABInRadians, double angularVelocityDAB)
{
 // Solve angles
 boolean isAHittingBounds = updateAnglesGivenAngleDAB(angleDABInRadians);
 // Solve angular velocity
 double A = clamp(angleDABInRadians, minA.getDoubleValue(), maxA.getDoubleValue());
 double dAdT = angularVelocityDAB;
 double e = unknownTriangleSideLengthByLawOfCosine(lengthAD.getDoubleValue(), lengthBA.getDoubleValue(), A);
 double eDot = lengthAD.getDoubleValue() * lengthBA.getDoubleValue() * sin(A) * dAdT / e;
 double dCdT = FourbarCalculatorTools.getAngleDotWithCosineLaw(lengthCB.getDoubleValue(), lengthDC.getDoubleValue(), 0.0, e, eDot);
 double angleDotDBA = FourbarCalculatorTools.getAngleDotWithCosineLaw(lengthBA.getDoubleValue(), e, eDot, lengthAD.getDoubleValue(), 0.0);
 double angleDotDBC = FourbarCalculatorTools.getAngleDotWithCosineLaw(lengthCB.getDoubleValue(), e, eDot, lengthDC.getDoubleValue(), 0.0);
 double dBdT = angleDotDBA + angleDotDBC;
 double dDdT = -dAdT - dBdT - dCdT;
 this.angleDtDAB = dAdT;
 this.angleDtABC = dBdT;
 this.angleDtBCD = dCdT;
 this.angleDtCDA = dDdT;
 return isAHittingBounds;
}

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

@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testRandomQuadrilatteral()
{
 YoVariableSideFourbarCalculatorWithDerivatives calculator = new YoVariableSideFourbarCalculatorWithDerivatives("testCalculator", registry);
 Random rand = new Random(1986L);
 for (int i = 0; i < 10000; i++)
 {
   double e = 100 * (rand.nextDouble() + 0.001);
   double k1 = rand.nextDouble();
   double k2 = rand.nextDouble();
   double d1 = e * abs(rand.nextGaussian());
   double d2 = e * abs(rand.nextGaussian());
   double DE = e * k1, DF = e * k2, BE = e * (1 - k1), BF = e * (1 - k2);
   double AE = d1, CF = d2;
   double AD = sqrt(DE * DE + AE * AE), DAE = atan2(DE, AE), ADE = atan2(AE, DE);
   double AB = sqrt(AE * AE + BE * BE), BAE = atan2(BE, AE), ABE = atan2(AE, BE);
   double CD = sqrt(CF * CF + DF * DF), CDF = atan2(CF, DF), DCF = atan2(DF, CF);
   double BC = sqrt(BF * BF + CF * CF), CBF = atan2(CF, BF), BCF = atan2(BF, CF);
   double BAD = DAE + BAE, ABC = ABE + CBF, BCD = BCF + DCF, ADC = ADE + CDF;
   solveFourBar(calculator, AD, AB, BC, CD, BAD, ABC, BCD, ADC);
 }
}

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

public void setSideLengths(double lengthAD, double lengthBA, double lengthCB, double lengthDC)
{
 this.lengthAD.set(lengthAD);
 this.lengthBA.set(lengthBA);
 this.lengthCB.set(lengthCB);
 this.lengthDC.set(lengthDC);
 double eMax = min(lengthAD + lengthBA, lengthDC + lengthCB);
 if (eMax == lengthAD + lengthBA)
   maxA.set(PI);
 else
   maxA.set(getAngleWithCosineLaw(lengthAD, lengthBA, eMax));
 double fMax = min(lengthAD + lengthDC, lengthBA + lengthCB);
 if (fMax == lengthAD + lengthDC)
   minA.set(getAngleWithCosineLaw(fMax, lengthBA, lengthCB));
 else
   minA.set(getAngleWithCosineLaw(fMax, lengthAD, lengthDC));
}

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

public boolean updateAnglesVelocitiesAndAccelerationsGivenAngleDAB(double angleDABInRadians, double angularVelocityDAB, double angularAccelerationDAB)
{
 // Solve angles and angular velocity
 boolean isAHittingBounds = updateAnglesAndVelocitiesGivenAngleDAB(angleDABInRadians, angularVelocityDAB);
 // Solve angular acceleration
 double A = clipToMinMax(angleDABInRadians, minA.getDoubleValue(), maxA.getDoubleValue());
 double dAdT = angularVelocityDAB;
 double dAdT2 = angularAccelerationDAB;
 double e = getUnknownTriangleSideLengthByLawOfCosine(lengthAD.getDoubleValue(), lengthBA.getDoubleValue(), A);
 double eDot = lengthAD.getDoubleValue() * lengthBA.getDoubleValue() * sin(A) * dAdT / e;
 double eDDot = lengthAD.getDoubleValue() * lengthBA.getDoubleValue() / e * (cos(A) * dAdT * dAdT + sin(A) * (dAdT2 - eDot * dAdT / e));
 double dCdT2 = FourbarCalculatorTools.getAngleDDotWithCosineLaw(lengthCB.getDoubleValue(), lengthDC.getDoubleValue(), 0.0, 0.0, e, eDot, eDDot);
 double angleDDotDBA = FourbarCalculatorTools.getAngleDDotWithCosineLaw(lengthBA.getDoubleValue(), e, eDot, eDDot, lengthAD.getDoubleValue(), 0.0, 0.0);
 double angleDDotDBC = FourbarCalculatorTools.getAngleDDotWithCosineLaw(lengthCB.getDoubleValue(), e, eDot, eDDot, lengthDC.getDoubleValue(), 0.0, 0.0);
 double dBdT2 = angleDDotDBA + angleDDotDBC;
 double dDdT2 = -dAdT2 - dBdT2 - dCdT2;
 this.angleDt2DAB = dAdT2;
 this.angleDt2ABC = dBdT2;
 this.angleDt2BCD = dCdT2;
 this.angleDt2CDA = dDdT2;
 return isAHittingBounds;
}

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

/**
* Compute every angle of the quadrilateral.
* @param angleDABInRadians is the angle formed by the sides lengthAD and lengthBA (see scheme in this class)
* @param angularVelocityDAB first time-derivative of the angle DAB
* @return true if the angle DAB is out of range making the quadrilateral non-convex
*/
public boolean updateAnglesAndVelocitiesGivenAngleDAB(double angleDABInRadians, double angularVelocityDAB)
{
 // Solve angles
 boolean isAHittingBounds = updateAnglesGivenAngleDAB(angleDABInRadians);
 // Solve angular velocity
 double A = clipToMinMax(angleDABInRadians, minA.getDoubleValue(), maxA.getDoubleValue());
 double dAdT = angularVelocityDAB;
 double e = getUnknownTriangleSideLengthByLawOfCosine(lengthAD.getDoubleValue(), lengthBA.getDoubleValue(), A);
 double eDot = lengthAD.getDoubleValue() * lengthBA.getDoubleValue() * sin(A) * dAdT / e;
 double dCdT = FourbarCalculatorTools.getAngleDotWithCosineLaw(lengthCB.getDoubleValue(), lengthDC.getDoubleValue(), 0.0, e, eDot);
 double angleDotDBA = FourbarCalculatorTools.getAngleDotWithCosineLaw(lengthBA.getDoubleValue(), e, eDot, lengthAD.getDoubleValue(), 0.0);
 double angleDotDBC = FourbarCalculatorTools.getAngleDotWithCosineLaw(lengthCB.getDoubleValue(), e, eDot, lengthDC.getDoubleValue(), 0.0);
 double dBdT = angleDotDBA + angleDotDBC;
 double dDdT = -dAdT - dBdT - dCdT;
 this.angleDtDAB = dAdT;
 this.angleDtABC = dBdT;
 this.angleDtBCD = dCdT;
 this.angleDtCDA = dDdT;
 return isAHittingBounds;
}

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

public void computeMasterJointAngleGivenAngleABC(double angleABCInRadians)
{
 double B = angleABCInRadians;
 double f = getUnknownTriangleSideLengthByLawOfCosine(lengthBA.getDoubleValue(), lengthCB.getDoubleValue(), B);
 double D = getAngleWithCosineLaw(lengthDC.getDoubleValue(), lengthAD.getDoubleValue(), f);
 double angleACB = getAngleWithCosineLaw(lengthCB.getDoubleValue(), f, lengthBA.getDoubleValue());
 double angleACD = getAngleWithCosineLaw(lengthDC.getDoubleValue(), f, lengthAD.getDoubleValue());
 double C = angleACB + angleACD;
 double A = 2 * PI - D - B - C;
 this.angleDAB = A;
}

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

private static double getAngleWithCosineLaw(double l_neighbour1, double l_neighbour2, double l_opposite)
{
 double angle = Math.acos(getCosineAngleWithCosineLaw(l_neighbour1, l_neighbour2, l_opposite));
 return angle;
}

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

public void computeMasterJointAngleGivenAngleABC(double angleABCInRadians)
{
 double B = angleABCInRadians;
 double f = unknownTriangleSideLengthByLawOfCosine(lengthBA.getDoubleValue(), lengthCB.getDoubleValue(), B);
 double D = getAngleWithCosineLaw(lengthDC.getDoubleValue(), lengthAD.getDoubleValue(), f);
 double angleACB = getAngleWithCosineLaw(lengthCB.getDoubleValue(), f, lengthBA.getDoubleValue());
 double angleACD = getAngleWithCosineLaw(lengthDC.getDoubleValue(), f, lengthAD.getDoubleValue());
 double C = angleACB + angleACD;
 double A = 2 * PI - D - B - C;
 this.angleDAB = A;
}

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

/**
* Takes in angle D and computes the value of the master joint angle.
* Same notation used in the rest of the class.
*/
public void computeMasterJointAngleGivenAngleCDA(double angleCDAInRadians)
{
 double D = angleCDAInRadians;
 double f = getUnknownTriangleSideLengthByLawOfCosine(lengthDC.getDoubleValue(), lengthAD.getDoubleValue(), D);
 double angleCAD = getAngleWithCosineLaw(lengthAD.getDoubleValue(), f, lengthDC.getDoubleValue());
 double angleCAB = getAngleWithCosineLaw(lengthBA.getDoubleValue(), f, lengthCB.getDoubleValue());
 double A = angleCAD + angleCAB;
 this.angleDAB = A;
}

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

/**
* Takes in angle D and computes the value of the master joint angle.
* Same notation used in the rest of the class.
*/
public void computeMasterJointAngleGivenAngleCDA(double angleCDAInRadians)
{
 double D = angleCDAInRadians;
 double f = unknownTriangleSideLengthByLawOfCosine(lengthDC.getDoubleValue(), lengthAD.getDoubleValue(), D);
 double angleCAD = getAngleWithCosineLaw(lengthAD.getDoubleValue(), f, lengthDC.getDoubleValue());
 double angleCAB = getAngleWithCosineLaw(lengthBA.getDoubleValue(), f, lengthCB.getDoubleValue());
 double A = angleCAD + angleCAB;
 this.angleDAB = A;
}

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

public void computeMasterJointAngleGivenAngleBCD(double angleBCDInRadians)
{
 double C = angleBCDInRadians;
 double e = getUnknownTriangleSideLengthByLawOfCosine(lengthCB.getDoubleValue(), lengthDC.getDoubleValue(), C);
 double A = getAngleWithCosineLaw(lengthAD.getDoubleValue(), lengthBA.getDoubleValue(), e);
 this.angleDAB = A;
}

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

public void computeMasterJointAngleGivenAngleBCD(double angleBCDInRadians)
{
 double C = angleBCDInRadians;
 double e = unknownTriangleSideLengthByLawOfCosine(lengthCB.getDoubleValue(), lengthDC.getDoubleValue(), C);
 double A = getAngleWithCosineLaw(lengthAD.getDoubleValue(), lengthBA.getDoubleValue(), e);
 this.angleDAB = A;
}

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

YoVariableSideFourbarCalculatorWithDerivatives

相关文章

    查看更多

    热门标签

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

    最新文章

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