45 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)
66 if (jacobian.rows() !=
kinematics[0].jacobian.rows() * 6 || jacobian.cols() !=
kinematics[0].jacobian(0).data.cols())
ThrowNamed(
"Wrong size of jacobian! " <<
kinematics[0].jacobian(0).data.cols());
67 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
71 jacobian.middleRows<6>(i * 6) =
kinematics[0].jacobian[i].data;
78 if (jacobian.rows() !=
kinematics[0].jacobian.rows() * 6 || jacobian.cols() !=
kinematics[0].jacobian(0).data.cols())
ThrowNamed(
"Wrong size of jacobian! " <<
kinematics[0].jacobian(0).data.cols());
79 for (
int i = 0; i <
kinematics[0].Phi.rows(); ++i)
83 jacobian.middleRows<6>(i * 6) =
kinematics[0].jacobian[i].data;
85 for (
int j = 0; j < 6; ++j)
87 hessian(i * 6 + j).block(0, 0, jacobian.cols(), jacobian.cols()) =
kinematics[0].hessian[i](j);
const RotationType & get_rotation_type() const
Eigen::Ref< Eigen::VectorXd > VectorXdRef
RotationType GetRotationTypeFromString(const std::string &rotation_type)
void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override
RotationType rotation_type_
void Instantiate(const EffFrameInitializer &init) override
std::vector< TaskVectorEntry > GetLieGroupIndices() override
const Eigen::Ref< const Eigen::VectorXd > & VectorXdRefConst
Eigen::VectorXd SetRotation(const KDL::Rotation &data, RotationType type)
REGISTER_TASKMAP_TYPE("EffFrame", exotica::EffFrame)
int GetRotationTypeLength(const RotationType &type)
Eigen::Ref< Hessian > HessianRef
Eigen::Ref< Eigen::MatrixXd > MatrixXdRef
int TaskSpaceDim() override
int TaskSpaceJacobianDim() override