Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 21 additions & 1 deletion include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/**
(C) Copyright 2020-2022 DQ Robotics Developers
(C) Copyright 2011-2026 DQ Robotics Developers

This file is part of DQ Robotics.

Expand Down Expand Up @@ -65,6 +65,18 @@ class DQ_SerialWholeBody : public DQ_Kinematics
* @throws std::runtime_error If either index is outside the valid range.
*/
void _check_to_jth_link_of_ith_chain(const int& to_ith_chain, const int& to_jth_link) const;
/**
*
* @param q Combined configuration vector of the serial whole-body model.
* @param to_ith_chain Index of the last chain to be accounted for.
* @param to_jth_link Link index inside the last chain. Used only if @p partial_chain is true.
* @param partial_chain True to stop at @p to_jth_link, false to go up to the end of @p to_ith_chain.
* @return Raw pose Jacobian.
*/
MatrixXd _raw_pose_jacobian_by_chain(const VectorXd& q,
const int& to_ith_chain,
const int& to_jth_link,
const bool& partial_chain) const;
public:
/** @brief Deleted default constructor. */
DQ_SerialWholeBody()=delete;
Expand Down Expand Up @@ -170,6 +182,14 @@ class DQ_SerialWholeBody : public DQ_Kinematics
* @return Raw pose Jacobian up to the requested chain and link.
*/
MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const;
/**
* @brief Computes the raw pose Jacobian up to the end of a specific chain.
*
* @param q Combined configuration vector of the serial whole-body model.
* @param to_ith_chain Index of the last chain to be accounted for.
* @return Raw pose Jacobian up to the end of @p to_ith_chain.
*/
MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain) const;
/**
* @brief Computes the raw time derivative of the pose Jacobian up to a specific link inside a specific chain.
*
Expand Down
85 changes: 55 additions & 30 deletions src/robot_modeling/DQ_SerialWholeBody.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/**
(C) Copyright 2020-2023 DQ Robotics Developers
(C) Copyright 2011-2026 DQ Robotics Developers

This file is part of DQ Robotics.

Expand All @@ -21,6 +21,7 @@ This file is part of DQ Robotics.

2. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
- Fixed bug 61 (https://github.com/dqrobotics/cpp/issues/61) in pose_jacobian method.
- Fixed bug 75 (https://github.com/dqrobotics/cpp/issues/75) in pose_jacobian method.

*/

Expand Down Expand Up @@ -190,56 +191,80 @@ DQ DQ_SerialWholeBody::raw_fkm_by_chain(const VectorXd &q, const int &to_ith_cha
return raw_fkm_by_chain(q,to_ith_chain,chain_[to_ith_chain]->get_dim_configuration_space()-1);
}

MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_by_chain(const VectorXd &q, const int &to_ith_chain, const int &to_jth_link) const
MatrixXd DQ_SerialWholeBody::_raw_pose_jacobian_by_chain(const VectorXd &q,
const int &to_ith_chain,
const int &to_jth_link,
const bool &partial_chain) const
{
_check_q_vec(q);
_check_to_jth_link_of_ith_chain(to_ith_chain, to_jth_link);

int n = to_ith_chain+1; //Size of the partial or total chain
DQ x_0_to_n = raw_fkm_by_chain(q,to_ith_chain,to_jth_link);
const int n = to_ith_chain+1; //Size of the partial or total chain
const DQ x_0_to_n = partial_chain ? raw_fkm_by_chain(q,to_ith_chain,to_jth_link)
: raw_fkm_by_chain(q,to_ith_chain);
int q_counter = 0;

//Not the best implementation but similar to MATLAB
std::vector<MatrixXd> J_vector;
int cols = 0;
for(int i=0;i<n;i++)
{
int dim = chain_[i]->get_dim_configuration_space();
// Addressing the to_jth_link

const DQ x_0_to_iplus1 = raw_fkm_by_chain(q,i);
const DQ x_iplus1_to_n = conj(x_0_to_iplus1)*x_0_to_n;

const int dim = chain_.at(i)->get_dim_configuration_space();
const VectorXd q_iplus1 = q.segment(q_counter,dim);
q_counter += dim;

if(i==0)
const DQ x_0_to_iplus1 = (partial_chain && i == n-1) ? x_0_to_n : raw_fkm_by_chain(q,i);
const DQ x_iplus1_to_n = conj(x_0_to_iplus1)*x_0_to_n;

if(partial_chain && i == n-1)
{
J_vector.push_back(haminus8(x_iplus1_to_n)*chain_[i]->pose_jacobian(q_iplus1,dim-1));
DQ x_0_to_i;
if(i > 0)
{
int link_chain;
int link;
std::tie(link_chain,link) = get_chain_and_link_from_index(i-1);
if(std::dynamic_pointer_cast<DQ_HolonomicBase>(chain_.at(link_chain)))
x_0_to_i = raw_fkm_by_chain(q,link_chain);
else
x_0_to_i = raw_fkm_by_chain(q,link_chain,link);
}
else if(auto serial_manipulator = std::dynamic_pointer_cast<DQ_SerialManipulator>(chain_.at(0)))
x_0_to_i = serial_manipulator->get_reference_frame();
else
x_0_to_i = chain_.at(0)->fkm(q_iplus1);
J_vector.push_back(hamiplus8(x_0_to_i)*haminus8(x_iplus1_to_n)*chain_.at(i)->pose_jacobian(q_iplus1,to_jth_link));
}
else
{
if(i==n-1) //To address the last index in the chain, or partial chains
J_vector.push_back(hamiplus8(raw_fkm_by_chain(q,i-1))*haminus8(x_iplus1_to_n)*chain_[i]->pose_jacobian(q_iplus1,to_jth_link));
else
J_vector.push_back(hamiplus8(raw_fkm_by_chain(q,i-1))*haminus8(x_iplus1_to_n)*chain_[i]->pose_jacobian(q_iplus1,dim-1));
const DQ x_0_to_i = (i == 0) ? DQ(1) : raw_fkm_by_chain(q,i-1);
J_vector.push_back(hamiplus8(x_0_to_i)*haminus8(x_iplus1_to_n)*chain_.at(i)->pose_jacobian(q_iplus1));
}
cols += J_vector.at(i).cols();
}

MatrixXd J_pose(8,q_counter);
MatrixXd J_pose(8,cols);
int col_counter = 0;
for(int i=0;i<n;i++)
for(const MatrixXd& J : J_vector)
{
int q_counter = chain_[i]->get_dim_configuration_space();
MatrixXd current_J = J_vector[i];
for(int j=0;j<q_counter;j++)
{
J_pose.col(col_counter)=current_J.col(j);
col_counter+=1;
}
J_pose.middleCols(col_counter,J.cols()) = J;
col_counter += J.cols();
}
return J_pose;
}

MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_by_chain(const VectorXd &q, const int &to_ith_chain, const int &to_jth_link) const
{
_check_q_vec(q);
_check_to_jth_link_of_ith_chain(to_ith_chain, to_jth_link);

return _raw_pose_jacobian_by_chain(q,to_ith_chain,to_jth_link,true);
}

MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_by_chain(const VectorXd &q, const int &to_ith_chain) const
{
_check_q_vec(q);
_check_to_ith_chain(to_ith_chain);

return _raw_pose_jacobian_by_chain(q,to_ith_chain,chain_.at(to_ith_chain)->get_dim_configuration_space()-1,false);
}

MatrixXd DQ_SerialWholeBody::pose_jacobian(const VectorXd &q, const int &to_ith_link) const
{
int to_ith_chain;
Expand All @@ -251,7 +276,7 @@ MatrixXd DQ_SerialWholeBody::pose_jacobian(const VectorXd &q, const int &to_ith_

MatrixXd DQ_SerialWholeBody::pose_jacobian(const VectorXd &q) const
{
return pose_jacobian(q, get_dim_configuration_space()-1);
return hamiplus8(get_reference_frame())*raw_pose_jacobian_by_chain(q,chain_.size()-1);
}

//To be implemented.
Expand Down
Loading