39 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
41 phi.segment<3>(i * 3) = Eigen::Map<Eigen::Vector3d>(
kinematics[0].Phi(i).p.data);
48 if (jacobian.rows() !=
kinematics[0].jacobian.rows() * 3 || jacobian.cols() !=
kinematics[0].jacobian(0).data.cols())
ThrowNamed(
"Wrong size of jacobian! " <<
kinematics[0].jacobian(0).data.cols());
49 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
51 phi.segment<3>(i * 3) = Eigen::Map<Eigen::Vector3d>(
kinematics[0].Phi(i).p.data);
52 jacobian.middleRows<3>(i * 3) =
kinematics[0].jacobian[i].data.topRows<3>();
59 if (jacobian.rows() !=
kinematics[0].jacobian.rows() * 3 || jacobian.cols() !=
kinematics[0].jacobian(0).data.cols())
ThrowNamed(
"Wrong size of jacobian! " <<
kinematics[0].jacobian(0).data.cols());
60 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
62 phi.segment<3>(i * 3) = Eigen::Map<Eigen::Vector3d>(
kinematics[0].Phi(i).p.data);
63 jacobian.middleRows<3>(i * 3) =
kinematics[0].jacobian[i].data.topRows<3>();
65 for (
int j = 0; j < 3; ++j)
67 hessian(i * 3 + j).block(0, 0, jacobian.cols(), jacobian.cols()) =
kinematics[0].hessian[i](j);
Eigen::Ref< Eigen::VectorXd > VectorXdRef
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
REGISTER_TASKMAP_TYPE("EffPosition", exotica::EffPosition)
int TaskSpaceDim() override
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::Ref< Hessian > HessianRef
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef