Commit 36d2aa75 authored by jcarpent's avatar jcarpent
Browse files

[Model] Add time derivative quantity of the joint Jacobian J

parent 1d6fe57b
...@@ -83,6 +83,7 @@ namespace se3 ...@@ -83,6 +83,7 @@ namespace se3
.ADD_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)") .ADD_DATA_PROPERTY(std::vector<int>,parents_fromRow,"First previous non-zero row in M (used in Cholesky)")
.ADD_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"") .ADD_DATA_PROPERTY(std::vector<int>,nvSubtree_fromRow,"")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,J,"Jacobian of joint placement") .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,J,"Jacobian of joint placement")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,dJ,"Time variation of the Jacobian of joint placement (data.J).")
.ADD_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.") .ADD_DATA_PROPERTY(container::aligned_vector<SE3>,iMf,"Body placement wrt to algorithm end effector.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag, .ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix6x,Ag,
......
...@@ -456,6 +456,9 @@ namespace se3 ...@@ -456,6 +456,9 @@ namespace se3
/// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call se3::getJacobian /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call se3::getJacobian
Matrix6x J; Matrix6x J;
/// \brief Derivative of the Jacobian with respect to the time.
Matrix6x dJ;
/// \brief Vector of joint placements wrt to algorithm end effector. /// \brief Vector of joint placements wrt to algorithm end effector.
container::aligned_vector<SE3> iMf; container::aligned_vector<SE3> iMf;
......
...@@ -252,6 +252,7 @@ namespace se3 ...@@ -252,6 +252,7 @@ namespace se3
,parents_fromRow((std::size_t)model.nv) ,parents_fromRow((std::size_t)model.nv)
,nvSubtree_fromRow((std::size_t)model.nv) ,nvSubtree_fromRow((std::size_t)model.nv)
,J(6,model.nv) ,J(6,model.nv)
,dJ(6,model.nv)
,iMf((std::size_t)model.njoints) ,iMf((std::size_t)model.njoints)
,com((std::size_t)model.njoints) ,com((std::size_t)model.njoints)
,vcom((std::size_t)model.njoints) ,vcom((std::size_t)model.njoints)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment