45 N =
scene_->GetKinematicTree().GetNumControlledJoints();
55 if (phi.rows() !=
N)
ThrowNamed(
"Wrong size of phi!");
57 const Eigen::MatrixXd& limits =
scene_->GetKinematicTree().GetJointLimits();
58 const Eigen::VectorXd& low_limits = limits.col(0);
59 const Eigen::VectorXd& high_limits = limits.col(1);
60 const Eigen::VectorXd tau = 0.5 *
safe_percentage_ * (high_limits - low_limits);
63 phi = (q.array() < (low_limits + tau).array()).select(q - low_limits - tau, phi);
65 phi = (q.array() > (high_limits - tau).array()).select(q - high_limits + tau, phi);
70 if (jacobian.rows() !=
N || jacobian.cols() !=
N)
ThrowNamed(
"Wrong size of jacobian! " <<
N);
74 const Eigen::MatrixXd& limits =
scene_->GetKinematicTree().GetJointLimits();
75 const Eigen::VectorXd& low_limits = limits.col(0);
76 const Eigen::VectorXd& high_limits = limits.col(1);
77 const Eigen::VectorXd tau = 0.5 *
safe_percentage_ * (high_limits - low_limits);
78 for (
int i = 0; i <
N; i++)
80 if (q(i) >= low_limits(i) + tau(i) && q(i) <= high_limits(i) - tau(i))
93 if (hessian.size() !=
N)
ThrowNamed(
"Wrong size of hessian! " <<
N);
105 Update(x.head(
scene_->get_num_positions()), phi, dphi_dx.topLeftCorner(
N,
N));
111 Update(x.head(
scene_->get_num_positions()), phi, dphi_dx.topLeftCorner(
N,
N));
Implementation of joint limits task map. Note: we dont want to always stay at the centre of the joint...
Eigen::Ref< Eigen::VectorXd > VectorXdRef
int TaskSpaceDim() override
REGISTER_TASKMAP_TYPE("JointLimit", exotica::JointLimit)
std::shared_ptr< Scene > ScenePtr
JointLimitInitializer parameters_
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::Ref< Hessian > HessianRef
void Update(Eigen::VectorXdRefConst q, Eigen::VectorXdRef phi) override
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void AssignScene(ScenePtr scene) override