39 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
49 if (jacobian.rows() !=
kinematics[0].jacobian.rows() * 2 || jacobian.cols() !=
kinematics[0].jacobian(0).data.cols())
ThrowNamed(
"Wrong size of jacobian! " <<
kinematics[0].jacobian(0).data.cols());
50 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
54 jacobian.middleRows<2>(i * 2) =
kinematics[0].jacobian[i].data.topRows<2>();
61 if (jacobian.rows() !=
kinematics[0].jacobian.rows() * 2 || jacobian.cols() !=
kinematics[0].jacobian(0).data.cols())
ThrowNamed(
"Wrong size of jacobian! " <<
kinematics[0].jacobian(0).data.cols());
62 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
66 jacobian.middleRows<2>(i * 2) =
kinematics[0].jacobian[i].data.topRows<2>();
68 for (
int j = 0; j < 2; ++j)
70 hessian(i * 2 + j).block(0, 0, jacobian.cols(), jacobian.cols()) =
kinematics[0].hessian[i](j);
Eigen::Ref< Eigen::VectorXd > VectorXdRef
int TaskSpaceDim() override
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
REGISTER_TASKMAP_TYPE("EffPositionXY", exotica::EffPositionXY)
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
Eigen::Ref< Hessian > HessianRef
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef