Commit 33d7cce3 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[sot-dynamic-pinocchio][BUG] fix bug from prev pr. frameJacobian is already in world frame

parent 5732aad5
......@@ -742,12 +742,14 @@ computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res,co
//TODO: Find a way to remove tmp object
se3::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
//Computes Jacobian in world coordinates.
if(isFrame){
//se3::framesForwardKinematics(*m_model,*m_data);
se3::getJacobian<se3::LOCAL>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
se3::framesForwardKinematics(*m_model,*m_data);
se3::getFrameJacobian(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
}
else se3::getJacobian<se3::WORLD>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
else
se3::getJacobian<se3::WORLD>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
res = tmp;
sotDEBUGOUT(25);
return res;
......@@ -767,8 +769,8 @@ computeGenericEndeffJacobian(const bool isFrame, const int jointId,dg::Matrix& r
se3::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
//std::string temp;
if(isFrame){
//se3::framesForwardKinematics(*m_model,*m_data);
se3::getJacobian<se3::LOCAL>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
se3::framesForwardKinematics(*m_model,*m_data);
se3::getFrameJacobian(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
sotDEBUG(25) << "EndEffJacobian for "
<< m_model->frames.at((se3::Model::Index)jointId).name
<<" is "<<tmp<<std::endl;
......
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