40 for (std::size_t i = 0; i <
frames_.size(); ++i)
42 const int eff_id = 2 * i;
43 Eigen::Vector3d p = Eigen::Map<Eigen::Vector3d>(
kinematics[0].Phi(i).p.data);
45 phi(eff_id + 1) = -p(2);
54 for (std::size_t i = 0; i <
frames_.size(); ++i)
56 const int eff_id = 2 * i;
57 Eigen::Vector3d p = Eigen::Map<Eigen::Vector3d>(
kinematics[0].Phi(i).p.data);
59 phi(eff_id + 1) = -p(2);
61 for (
int j = 0; j < jacobian.cols(); ++j)
63 Eigen::Vector3d pd =
kinematics[0].jacobian[i].data.topRows<3>().col(j);
64 jacobian(eff_id, j) = 2.0 * (p(0) * pd(0) + p(1) * pd(1) - 2.0 *
tan_theta_squared_(i) * p(2) * pd(2));
65 jacobian(eff_id + 1, j) = -pd(2);
74 if (init.Theta.size() !=
static_cast<int>(
frames_.size()))
ThrowPretty(
"Incorrect size for Theta (expecting " <<
frames_.size() <<
").");
77 for (std::size_t i = 0; i <
frames_.size(); ++i)
79 const double tan_theta = std::tan(init.Theta[i]);
std::vector< KinematicFrameRequest > frames_
Eigen::Ref< Eigen::VectorXd > VectorXdRef
Keeps a given point within field of view of the end-effector.
Eigen::VectorXd tan_theta_squared_
The tangent squared of given viewing angle Theta.
GazeAtConstraintInitializer parameters_
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
void Instantiate(const GazeAtConstraintInitializer &init) override
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
int TaskSpaceDim() override
REGISTER_TASKMAP_TYPE("GazeAtConstraint", exotica::GazeAtConstraint)