50 Eigen::Vector3d p_t, p_t_prev;
51 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
55 phi(i) = (p_t_prev - p_t).norm();
66 Eigen::Vector3d p_t, p_t_prev;
67 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
71 phi(i) = (p_t - p_t_prev).norm();
75 jacobian.row(i) = ((p_t(0) - p_t_prev(0)) *
kinematics[0].jacobian[i].data.row(0) +
76 (p_t(1) - p_t_prev(1)) *
kinematics[0].jacobian[i].data.row(1) +
77 (p_t(2) - p_t_prev(2)) *
kinematics[0].jacobian[i].data.row(2)) /
Eigen::Ref< Eigen::VectorXd > VectorXdRef
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
int TaskSpaceDim() override
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix< double, 3, 1 > &e)
REGISTER_TASKMAP_TYPE("EffVelocity", exotica::EffVelocity)