private void checkConsistency(InverseDynamicsJoint joint, DenseMatrix64F desiredVelocity) { MathTools.checkIfEqual(joint.getDegreesOfFreedom(), desiredVelocity.getNumRows()); }
public void set(ReferenceFrame expressedInFrame, double[] doubleArray) { MathTools.checkIfEqual(doubleArray.length, SIZE); this.expressedInFrame = expressedInFrame; this.angularPart.set(doubleArray[0], doubleArray[1], doubleArray[2]); this.linearPart.set(doubleArray[3], doubleArray[4], doubleArray[5]); }
/** * Construct using a double array ([angular; linear]) */ public SpatialMotionVector(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, ReferenceFrame expressedInFrame, double[] array) { MathTools.checkIfEqual(SIZE, array.length); this.bodyFrame = bodyFrame; this.baseFrame = baseFrame; this.expressedInFrame = expressedInFrame; this.angularPart = new Vector3d(array[0], array[1], array[2]); this.linearPart = new Vector3d(array[3], array[4], array[5]); }
public void addArmTrajectoryPoint(double trajectoryPointTime, double[] desiredJointPositions, double[] desiredJointVelocities) { MathTools.checkIfEqual(desiredJointPositions.length, desiredJointVelocities.length); for (int jointIndex = 0; jointIndex < desiredJointPositions.length; jointIndex++) { SimpleTrajectoryPoint1DList jointTrajectoryInput = jointTrajectoryInputs.getAndGrowIfNeeded(jointIndex); jointTrajectoryInput.addTrajectoryPoint(trajectoryPointTime, desiredJointPositions[jointIndex], desiredJointVelocities[jointIndex]); } }
public void setQuadraticUsingInitialVelocityAndAcceleration(double t0, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 3); for (Direction direction : Direction.values) { polynomials.get(direction).setQuadraticUsingInitialAcceleration(t0, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction)); } setYoVariables(t0, tf); }
public void setOneDoFJointDesiredAcceleration(int jointIndex, double desiredVelocity) { MathTools.checkIfEqual(joints.get(jointIndex).getDegreesOfFreedom(), 1); desiredVelocities.get(jointIndex).reshape(1, 1); desiredVelocities.get(jointIndex).set(0, 0, desiredVelocity); }
public void setQuadraticUsingInitialVelocity(double t0, double tf, FramePoint p0, FrameVector pd0, FramePoint pf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 3); for (Direction direction : Direction.values) { polynomials.get(direction).setQuadratic(t0, tf, p0.get(direction), pd0.get(direction), pf.get(direction)); } setYoVariables(t0, tf); }
public void setQuadraticUsingFinalVelocity(double t0, double tf, FramePoint p0, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 3); for (Direction direction : Direction.values) { polynomials.get(direction).setQuadratic(tf, t0, pf.get(direction), pdf.get(direction), p0.get(direction)); } setYoVariables(t0, tf); }
public void setCubic(double t0, double tf, FramePoint p0, FrameVector pd0, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 4); for (Direction direction : Direction.values) { polynomials.get(direction).setCubic(t0, tf, p0.get(direction), pd0.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void setCubicUsingFinalAccelerationButNotFinalPosition(double t0, double tf, FramePoint p0, FrameVector pd0, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 4); for (Direction direction : Direction.values) { polynomials.get(direction).setCubicUsingFinalAccelerationButNotFinalPosition(t0, tf, p0.get(direction), pd0.get(direction), pdf.get(direction), pddf.get(direction)); } setYoVariables(t0, tf); }
public void setQuarticUsingInitialAcceleration(double t0, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 5); for (Direction direction : Direction.values) { polynomials.get(direction).setQuartic(t0, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void setQuarticUsingFinalAcceleration(double t0, double tf, FramePoint p0, FrameVector pd0, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 5); for (Direction direction : Direction.values) { polynomials.get(direction).setQuartic(tf, t0, pf.get(direction), pdf.get(direction), pddf.get(direction), p0.get(direction), pd0.get(direction)); } setYoVariables(t0, tf); }
public void setOneDoFJoint(int jointIndex, double desiredPosition, double desiredVelocity, double feedForwardAcceleration) { MathTools.checkIfEqual(joints.get(jointIndex).getDegreesOfFreedom(), 1); desiredPositions.get(jointIndex).setValue(desiredPosition); desiredVelocities.get(jointIndex).setValue(desiredVelocity); feedForwardAccelerations.get(jointIndex).setValue(feedForwardAcceleration); }
public void set(ReferenceFrame expressedInFrame, DenseMatrix64F matrix) { MathTools.checkIfEqual(matrix.getNumRows(), SIZE); MathTools.checkIfEqual(matrix.getNumCols(), 1); this.expressedInFrame = expressedInFrame; this.angularPart.set(matrix.get(0, 0), matrix.get(1, 0), matrix.get(2, 0)); this.linearPart.set(matrix.get(3, 0), matrix.get(4, 0), matrix.get(5, 0)); }
public void setQuarticUsingIntermediateVelocity(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pd1, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 5); for (Direction direction : Direction.values) { polynomials.get(direction).setQuarticUsingIntermediateVelocity(t0, t1, tf, p0.get(direction), pd0.get(direction), pd1.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void setQuintic(double t0, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 6); for (Direction direction : Direction.values) { polynomials.get(direction).setQuintic(t0, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), pf.get(direction), pdf.get(direction), pddf.get(direction)); } setYoVariables(t0, tf); }
public void setQuinticUsingIntermediateVelocityAndAcceleration(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pd1, FrameVector pdd1, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 6); for (Direction direction : Direction.values) { polynomials.get(direction).setQuinticUsingIntermediateVelocityAndAcceleration(t0, t1, tf, p0.get(direction), pd0.get(direction), pd1.get(direction), pdd1.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void setSexticUsingWaypoint(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FramePoint p1, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 7); for (Direction direction : Direction.values) { polynomials.get(direction).setSexticUsingWaypoint(t0, t1, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), p1.get(direction), pf.get(direction), pdf.get(direction), pddf.get(direction)); } setYoVariables(t0, tf); }
public void setSexticUsingWaypointVelocityAndAccelerationAndInitialAcceleration(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FrameVector pd1, FrameVector pdd1, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 7); for (Direction direction : Direction.values) { polynomials.get(direction).setSexticUsingWaypointVelocityAndAcceleration(t0, t1, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), pd1.get(direction), pdd1.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void setSexticUsingWaypointVelocityAndAccelerationAndFinalAcceleration(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pd1, FrameVector pdd1, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 7); for (Direction direction : Direction.values) { polynomials.get(direction).setSexticUsingWaypointVelocityAndAcceleration(tf, t1, t0, pf.get(direction), pdf.get(direction), pddf.get(direction), pd1.get(direction), pdd1.get(direction), p0.get(direction), pd0.get(direction)); } setYoVariables(t0, tf); }