24 #include <VirtualRobot/Robot.h>
49 inline Eigen::MatrixXf
51 const Eigen::Ref<const Eigen::VectorXf>& jointLimitsLow,
52 const Eigen::Ref<const Eigen::VectorXf>& jointLimitsHigh,
53 Eigen::VectorXf& jointAngleLimitGradient)
58 Eigen::VectorXf weights(nbJoints);
59 Eigen::VectorXf gradient = Eigen::VectorXf::Zero(nbJoints);
61 if (jointAngleLimitGradient.rows() == 0)
63 jointAngleLimitGradient = Eigen::VectorXf::Zero(nbJoints);
66 for (
int i = 0; i < nbJoints; i++)
68 if ((jointLimitsLow(i) == 0 && jointLimitsHigh(i) == 0) || jointLimitsHigh(i) > 1e8)
75 gradient(i) = std::pow(jointLimitsHigh(i) - jointLimitsLow(i), 2) *
76 (2 *
jointAngles(i) - jointLimitsHigh(i) - jointLimitsLow(i)) /
77 (4 * std::pow(jointLimitsHigh(i) -
jointAngles(i), 2) *
92 jointAngleLimitGradient = gradient;
93 return weights.asDiagonal();