44 std::vector<TaskVectorEntry> ret;
46 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
56 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
65 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());
66 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
69 jacobian.middleRows<3>(i * 3) =
kinematics[0].jacobian[i].data.bottomRows<3>();
76 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());
77 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
80 jacobian.middleRows<3>(i * 3) =
kinematics[0].jacobian[i].data.bottomRows<3>();
82 for (
int j = 0; j < 3; ++j)
84 hessian(i * 3 + j).block(0, 0, jacobian.cols(), jacobian.cols()) =
kinematics[0].hessian[i](j + 3);
RotationType rotation_type_
Eigen::Ref< Eigen::VectorXd > VectorXdRef
const RotationType & get_rotation_type() const
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
RotationType GetRotationTypeFromString(const std::string &rotation_type)
int TaskSpaceDim() override
void Instantiate(const EffOrientationInitializer &init) override
std::vector< TaskVectorEntry > GetLieGroupIndices() override
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
int TaskSpaceJacobianDim() override
Eigen::VectorXd SetRotation(const KDL::Rotation &data, RotationType type)
int GetRotationTypeLength(const RotationType &type)
Eigen::Ref< Hessian > HessianRef
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
REGISTER_TASKMAP_TYPE("EffOrientation", exotica::EffOrientation)