Commit 8d6251c5 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[sot-dynamic-pinocchio] fix getFrameJacobian

parent 54f27d22
......@@ -745,8 +745,8 @@ computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res,co
//Computes Jacobian in world coordinates.
if(isFrame){
se3::framesForwardKinematics(*m_model,*m_data);
se3::getFrameJacobian(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
se3::getJacobian<se3::WORLD>(*m_model,*m_data,
m_model->frames[(se3::Model::Index)jointId].parent,tmp);
}
else
se3::getJacobian<se3::WORLD>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
......@@ -768,6 +768,7 @@ computeGenericEndeffJacobian(const bool isFrame, const int jointId,dg::Matrix& r
//TODO: Find a way to remove tmp object
se3::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
//std::string temp;
//Computes Jacobian in end-eff coordinates.
if(isFrame){
se3::framesForwardKinematics(*m_model,*m_data);
se3::getFrameJacobian(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
......@@ -777,7 +778,7 @@ computeGenericEndeffJacobian(const bool isFrame, const int jointId,dg::Matrix& r
}
else {
//temp = m_model->getJointName((se3::Model::Index)jointId);
se3::getJacobian<se3::WORLD>
se3::getJacobian<se3::LOCAL>
(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
sotDEBUG(25) << "EndEffJacobian for "
<< m_model->getJointName((se3::Model::Index)jointId)
......
Markdown is supported
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