bool Equal(const Jacobian& a,const Jacobian& b,double eps)
{
if(a.rows()==b.rows()&&a.columns()==b.columns()){
return a.data.isApprox(b.data,eps);
}else
return false;
}
int ChainJntToJacDotSolver::JntToJacDot(const JntArrayVel& q_in, Jacobian& jdot, int seg_nr)
{
unsigned int segmentNr;
if(seg_nr<0)
segmentNr=chain.getNrOfSegments();
else
segmentNr = seg_nr;
//Initialize Jacobian to zero since only segmentNr columns are computed
SetToZero(jdot) ;
if(q_in.q.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jdot.columns())
return (error = E_JAC_DOT_FAILED);
else if(segmentNr>chain.getNrOfSegments())
return (error = E_JAC_DOT_FAILED);
// First compute the jacobian in the Hybrid representation
jac_solver_.JntToJac(q_in.q,jac_,segmentNr);
// Change the reference frame and/or the reference point
switch(representation_)
{
case HYBRID:
// Do Nothing as it is the default in KDL;
break;
case BODYFIXED:
// Ref Frame {ee}, Ref Point {ee}
fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr);
jac_.changeBase(F_bs_ee_.M.Inverse());
break;
case INTERTIAL:
// Ref Frame {bs}, Ref Point {bs}
fk_solver_.JntToCart(q_in.q,F_bs_ee_,segmentNr);
jac_.changeRefPoint(-F_bs_ee_.p);
break;
default:
return (error = E_JAC_DOT_FAILED);
}
// Let's compute Jdot in the corresponding representation
int k=0;
for(unsigned int i=0;i<segmentNr;++i)
{
//Only increase joint nr if the segment has a joint
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
for(unsigned int j=0;j<chain.getNrOfJoints();++j)
{
// Column J is the sum of all partial derivatives ref (41)
if(!locked_joints_[j])
jac_dot_k_ += getPartialDerivative(jac_,j,k,representation_) * q_in.qdot(j);
}
jdot.setColumn(k++,jac_dot_k_);
SetToZero(jac_dot_k_);
}
}
return (error = E_NOERROR);
}
bool changeBase(const Jacobian& src1, const Rotation& rot, Jacobian& dest)
{
if(src1.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src1.columns();i++)
dest.setColumn(i,rot*src1.getColumn(i));;
return true;
}
bool changeRefFrame(const Jacobian& src1,const Frame& frame, Jacobian& dest)
{
if(src1.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src1.columns();i++)
dest.setColumn(i,frame*src1.getColumn(i));
return true;
}
bool changeRefPoint(const Jacobian& src1, const Vector& base_AB, Jacobian& dest)
{
if(src1.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src1.columns();i++)
dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB));
return true;
}
bool multiplyInertiaJacobian(const Jacobian& src, const RigidBodyInertia& I, MomentumJacobian& dest)
{
if(src.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src.columns();i++)
dest.setColumn(i,I*src.getColumn(i));;
return true;
}
bool divideJacobianInertia(const MomentumJacobian& src, const RigidBodyInertia& I, Jacobian& dest)
{
/** \todo if the inertia matrix is singular ? */
if(src.columns()!=dest.columns() || I.getMass() == 0)
return false;
for(unsigned int i=0;i<src.columns();i++)
dest.setColumn(i,src.getColumn(i)/I);
return true;
}
bool Equal(const Jacobian& a,const Jacobian& b,double eps)
{
if(a.rows()==b.rows()&&a.columns()==b.columns()){
bool rc=true;
for(unsigned int i=0;i<a.columns();i++)
rc&=Equal(a.twists[i],b.twists[i],eps);
return rc;
}else
return false;
}
void TranSVD::jacobian_reverse(Jacobian &jac)
{
Transformable &data = jac.base_numeric_parameters;
Eigen::SparseMatrix<double> old_matrix = jac.get_matrix(jac.observation_list(), super_parameter_names);
Eigen::SparseMatrix<double> base_jacobian;
base_jacobian = old_matrix * Vt;
jac.matrix = base_jacobian;
jac.base_numeric_par_names = base_parameter_names;
reverse(data);
}
void TranSVD::jacobian_forward(Jacobian &jac)
{
Transformable &data = jac.base_numeric_parameters;
Eigen::SparseMatrix<double> old_matrix = jac.get_matrix(jac.observation_list(), base_parameter_names);
Eigen::SparseMatrix<double> super_jacobian;
super_jacobian = old_matrix * Vt.transpose();
jac.matrix = super_jacobian;
jac.base_numeric_par_names = super_parameter_names;
forward(data);
}
void Jdot_diff(const Jacobian& J_q,
const Jacobian& J_qdt,
const double& dt,
Jacobian& Jdot)
{
assert(J_q.columns() == J_qdt.columns());
assert(J_q.columns() == Jdot.columns());
for(int l=0;l<6;l++)
for(int c=0;c<J_q.columns();c++)
Jdot(l,c) = (J_qdt(l,c) - J_q(l,c))/dt;
}
int ChainJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, int seg_nr)
{
unsigned int segmentNr;
if(seg_nr<0)
segmentNr=chain.getNrOfSegments();
else
segmentNr = seg_nr;
//Initialize Jacobian to zero since only segmentNr colunns are computed
SetToZero(jac) ;
if(q_in.rows()!=chain.getNrOfJoints()||nr_of_unlocked_joints_!=jac.columns())
return -1;
else if(segmentNr>chain.getNrOfSegments())
return -1;
T_tmp = Frame::Identity();
SetToZero(t_tmp);
int j=0;
int k=0;
Frame total;
for (unsigned int i=0;i<segmentNr;i++) {
//Calculate new Frame_base_ee
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
//pose of the new end-point expressed in the base
total = T_tmp*chain.getSegment(i).pose(q_in(j));
//changing base of new segment's twist to base frame if it is not locked
//t_tmp = T_tmp.M*chain.getSegment(i).twist(1.0);
if(!locked_joints_[j])
t_tmp = T_tmp.M*chain.getSegment(i).twist(q_in(j),1.0);
}else{
total = T_tmp*chain.getSegment(i).pose(0.0);
}
//Changing Refpoint of all columns to new ee
changeRefPoint(jac,total.p-T_tmp.p,jac);
//Only increase jointnr if the segment has a joint
if(chain.getSegment(i).getJoint().getType()!=Joint::None){
//Only put the twist inside if it is not locked
if(!locked_joints_[j])
jac.setColumn(k++,t_tmp);
j++;
}
T_tmp = total;
}
return 0;
}
void getFloatingBaseJacobianLoop(const UndirectedTree & undirected_tree,
const GeneralizedJntPositions &q,
const Traversal & traversal,
const int link_index,
Jacobian & jac)
{
Frame T_total = Frame::Identity(); //The transformation between link_index frame and current_link frame
assert(link_index < (int)undirected_tree.getNrOfLinks());
KDL::CoDyCo::LinkMap::const_iterator current_link;
current_link = undirected_tree.getLink(link_index);
//All the columns not modified are zero
SetToZero(jac);
KDL::CoDyCo::LinkMap::const_iterator parent_link=traversal.getParentLink(current_link);
while(current_link != traversal.getBaseLink()) {
double joint_pos = 0.0;
if( current_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
KDL::Twist jac_col;
int dof_index = current_link->getAdjacentJoint(parent_link)->getDOFIndex();
joint_pos = q.jnt_pos(dof_index);
KDL::Twist S_current_parent = parent_link->S(current_link,joint_pos);
jac_col = T_total*S_current_parent;
assert(6+dof_index < (int)jac.columns());
assert( dof_index < (int)undirected_tree.getNrOfDOFs() );
jac.setColumn(6+dof_index,jac_col);
}
KDL::Frame X_current_parent = parent_link->pose(current_link,joint_pos);
T_total = T_total*X_current_parent;
current_link = parent_link;
parent_link = traversal.getParentLink(current_link);
}
//Setting the floating part of the Jacobian
T_total = T_total*KDL::Frame(q.base_pos.M.Inverse());
jac.data.block(0,0,6,6) = TwistTransformationMatrix(T_total);
jac.changeBase(T_total.M.Inverse());
}
int TreeIdSolver_Vereshchagin::CartToJnt(const JntArray& q, const JntArray& q_dot, JntArray& q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques)
{
//Check sizes always
if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj)// || f_ext.size() != ns)
{
std::cout << "Error 1" << std::endl;
return -1;
}
if (alfa.columns() != nc || beta.rows() != nc)
// if (alfas.size() != nc || betas.size() != nc) // number of constraints should also become a vector. because nc defines a number of columns in constraint jacobian matrix A(alfa)
{
std::cout << "Error 2" << std::endl;
return -2;
}
//do an upward recursion for position and velocities
this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext);
//do an inward recursion for inertia, forces and constraints
this->downwards_sweep(alfa, torques);
//Solve for the constraint forces
// this->constraint_calculation(beta);
//do an upward recursion to propagate the result
// this->final_upwards_sweep(q_dotdot, torques);
return 0;
}
void changeRepresentation(Jacobian& J,const Frame& F_bs_ee,const int& representation)
{
switch(representation)
{
case ChainJntToJacDotSolver::HYBRID:
break;
case ChainJntToJacDotSolver::BODYFIXED:
// Ref Frame {ee}, Ref Point {ee}
J.changeBase(F_bs_ee.M.Inverse());
break;
case ChainJntToJacDotSolver::INTERTIAL:
// Ref Frame {bs}, Ref Point {bs}
J.changeRefPoint(-F_bs_ee.p);
break;
}
}
int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac, const std::string& segmentname) {
//First we check all the sizes:
if (q_in.rows() != tree.getNrOfJoints() || jac.columns() != tree.getNrOfJoints())
return -1;
//Lets search the tree-element
SegmentMap::const_iterator it = tree.getSegments().find(segmentname);
//If segmentname is not inside the tree, back out:
if (it == tree.getSegments().end())
return -2;
//Let's make the jacobian zero:
SetToZero(jac);
SegmentMap::const_iterator root = tree.getRootSegment();
Frame T_total = Frame::Identity();
//Lets recursively iterate until we are in the root segment
while (it != root) {
//get the corresponding q_nr for this TreeElement:
unsigned int q_nr = it->second.q_nr;
//get the pose of the segment:
Frame T_local = it->second.segment.pose(q_in(q_nr));
//calculate new T_end:
T_total = T_local * T_total;
//get the twist of the segment:
if (it->second.segment.getJoint().getType() != Joint::None) {
Twist t_local = it->second.segment.twist(q_in(q_nr), 1.0);
//transform the endpoint of the local twist to the global endpoint:
t_local = t_local.RefPoint(T_total.p - T_local.p);
//transform the base of the twist to the endpoint
t_local = T_total.M.Inverse(t_local);
//store the twist in the jacobian:
jac.setColumn(q_nr,t_local);
}//endif
//goto the parent
it = it->second.parent;
}//endwhile
//Change the base of the complete jacobian from the endpoint to the base
changeBase(jac, T_total.M, jac);
return 0;
}//end JntToJac
/// Sets the Jacobian, which is wi at any point.
void PoldiSpectrumLinearBackground::functionDeriv1DSpectrum(
const FunctionDomain1DSpectrum &domain, Jacobian &jacobian) {
double wsIndexDouble = static_cast<double>(domain.getWorkspaceIndex());
for (size_t i = 0; i < domain.size(); ++i) {
jacobian.set(i, 0, wsIndexDouble);
}
}
Jacobian::Jacobian(const Jacobian& arg):
size(arg.columns()),
nr_blocks(arg.nr_blocks)
{
twists = new Twist[size*nr_blocks];
for(unsigned int i=0;i<size*nr_blocks;i++)
twists[i] = arg.twists[i];
}
void MultiplyJacobian(const Jacobian& jac, const JntArray& src, Twist& dest)
{
assert(jac.columns()==src.size);
SetToZero(dest);
for(unsigned int i=0;i<6;i++)
for(unsigned int j=0;j<src.size;j++)
dest(i)+=jac(i,j)*src.data[j];
}
int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
const std::string& segmentname) {
//First we check all the sizes:
if (q_in.rows() != tree.getNrOfJoints() || jac.columns()
!= tree.getNrOfJoints())
return -1;
//Lets search the tree-element
SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname);
//If segmentname is not inside the tree, back out:
if (!it)
return -2;
//Let's make the jacobian zero:
SetToZero(jac);
SegmentMap::value_type const* root = tree.getSegmentPtr("root");
Frame T_total = Frame::Identity();
Frame T_local, T_joint;
Twist t_local;
//Lets recursively iterate until we are in the root segment
while (it != root) {
//get the corresponding q_nr for this TreeElement:
unsigned int q_nr = it->second.q_nr;
//get the pose of the joint.
T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr));
// combine with the tip to have the tip pose
T_local = T_joint*it->second.segment.getFrameToTip();
//calculate new T_end:
T_total = T_local * T_total;
//get the twist of the segment:
int ndof = it->second.segment.getJoint().getNDof();
for (int dof=0; dof<ndof; dof++) {
// combine joint rotation with tip position to get a reference frame for the joint
T_joint.p = T_local.p;
// in which the twist can be computed (needed for NDof joint)
t_local = it->second.segment.twist(T_joint, 1.0, dof);
//transform the endpoint of the local twist to the global endpoint:
t_local = t_local.RefPoint(T_total.p - T_local.p);
//transform the base of the twist to the endpoint
t_local = T_total.M.Inverse(t_local);
//store the twist in the jacobian:
jac.twists[q_nr+dof] = t_local;
}
//goto the parent
it = it->second.parent;
}//endwhile
//Change the base of the complete jacobian from the endpoint to the base
changeBase(jac, T_total.M, jac);
return 0;
}//end JntToJac
void TranFixed::jacobian_reverse(Jacobian &jac)
{
Transformable &data = jac.base_numeric_parameters;
set<string> new_pars;
for (auto &i : items)
{
new_pars.insert(i.first);
}
jac.add_cols(new_pars);
reverse(data);
}
void TranFixed::jacobian_forward(Jacobian &jac)
{
Transformable &data = jac.base_numeric_parameters;
set<string> rm_par_set;
for (const auto &irec : items)
{
rm_par_set.insert(irec.first);
}
//remove Fixed parameters from base_parameter_names
jac.remove_cols(rm_par_set);
forward(data);
}
inline Twist operator*(const Jacobian& jac, const JntArray& qdot)
{
assert(jac.columns() == qdot.rows());
Twist t;
int naxes = qdot.rows();
for(int i=0; i < 6; ++i)
for(int j=0; j < naxes; ++j)
t(i) += jac(i,j)*qdot(j);
return t;
}
void
IntrepidManager::FaceNormal::
operator()(Jacobian& J, int i_face, CellTopology& topo)
{
// FIXME - don't instantiate this, since Intrepid isn't completely free of its own MDArray, FieldContainer
#if 0
MDArray J_mda;
J.copyTo(J_mda);
MDArray fn_mda(m_im.m_Elements_Tag.num, m_im.m_Cub_Points_Tag.num, m_im.m_Spatial_Dim_Tag.num);
CellTools<double>::getPhysicalFaceNormals(fn_mda, J_mda, i_face, cell_topo);
this->copyFrom(fn_mda);
#endif
//CellTools<double>::getPhysicalFaceNormals(*this, jac, i_face, topo);
throw std::runtime_error(" don't instantiate this, since Intrepid isn't completely free of its own MDArray, FieldContainer");
}
void TranNormalize::jacobian_reverse(Jacobian &jac)
{
size_t icol = 0;
double factor = 0;
Transformable &data = jac.base_numeric_parameters;
unordered_map<string, int> par_2_col_map = jac.get_par2col_map();
auto iter_end = par_2_col_map.end();
for (const auto &irec : items)
{
auto iter = par_2_col_map.find(irec.first);
if (iter != iter_end)
{
icol = iter->second;
factor = irec.second.scale;
jac.matrix.col(icol) *= factor;
}
}
reverse(data);
}
void TranLog10::jacobian_reverse(Jacobian &jac)
{
size_t icol = 0;
double factor = 0;
double d = 0;
Transformable &data = jac.base_numeric_parameters;
reverse(data);
unordered_map<string, int> par_2_col_map = jac.get_par2col_map();
auto iter_end = par_2_col_map.end();
for (const auto &ipar : items)
{
auto iter = par_2_col_map.find(ipar);
if (iter != iter_end)
{
d = data.get_rec(ipar);
icol = iter->second;
factor = 1.0 / (d * log(10.0));
jac.matrix.col(icol) *= factor;
}
}
}
void getRelativeJacobianLoop(const UndirectedTree & undirected_tree,
const KDL::JntArray &q,
const Traversal & traversal,
const int link_index,
Jacobian & jac)
{
Frame T_total = Frame::Identity(); //The transformation between link_index frame and current_link frame
KDL::CoDyCo::LinkMap::const_iterator current_link;
current_link = undirected_tree.getLink(link_index);
//All the columns not modified are zero
SetToZero(jac);
KDL::CoDyCo::LinkMap::const_iterator parent_link=traversal.getParentLink(current_link);
while(current_link != traversal.getBaseLink()) {
double joint_pos = 0.0;
if( current_link->getAdjacentJoint(parent_link)->getNrOfDOFs() == 1 ) {
KDL::Twist jac_col;
int dof_index = current_link->getAdjacentJoint(parent_link)->getDOFIndex();
joint_pos = q(dof_index);
KDL::Twist S_current_parent = parent_link->S(current_link,joint_pos);
jac_col = T_total*S_current_parent;
jac.setColumn(dof_index,jac_col);
}
KDL::Frame X_current_parent = parent_link->pose(current_link,joint_pos);
T_total = T_total*X_current_parent;
current_link = parent_link;
parent_link = traversal.getParentLink(current_link);
}
}
/**
* Overridden Jacobian::get(...).
* @param iY :: The index of the data point
* @param iP :: The parameter index of an individual function.
*/
double get(size_t iY, size_t iP)
{
return m_J->get(m_iY0 + iY,iP);
}
/**
* Overridden Jacobian::set(...).
* @param iY :: The index of the data point
* @param iP :: The parameter index of an individual function.
* @param value :: The derivative value
*/
void set(size_t iY, size_t iP, double value)
{
m_J->set(m_iY0 + iY,iP,value);
}
void Display( void )
{
DoUpdateStep();
float scale2; /* real glui scale factor */
glutSetWindow( GrWindow );
glDrawBuffer( GL_BACK );
glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
glEnable( GL_DEPTH_TEST );
glShadeModel( GL_FLAT );
glMatrixMode( GL_PROJECTION );
glLoadIdentity();
if (WhichProjection == ORTHO)
glOrtho(-3., 3., -1.7, 1.7, 0.1, 1000.);
//glOrtho(-3., 3., -3., 3., 0.1, 1000.);
else
gluPerspective(75., 1., 0.1, 1000.);
// gluPerspective(90., 1., 0.1, 1000.);
glMatrixMode( GL_MODELVIEW );
glLoadIdentity();
gluLookAt( -3., 0., 3., 0., 0., 0., 0., 1., 0. );
glTranslatef( TransXYZ[0], TransXYZ[1], -TransXYZ[2] );
glRotatef( Yrot, 0., 1., 0. );
glRotatef( Xrot, 1., 0., 0. );
glMultMatrixf( (const GLfloat *) RotMatrix );
glScalef( Scale, Scale, Scale );
scale2 = 1. + Scale2; /* because glui translation starts at 0. */
if( scale2 < MINSCALE )
scale2 = MINSCALE;
glScalef( scale2, scale2, scale2 );
if( AxesOn ) {
glDisable(GL_LIGHTING);
glCallList( AxesList );
glEnable(GL_LIGHTING);
}
GLUI_Master.set_glutIdleFunc( Animate );
DrawTarget(T);
if (WhichMethod != COMPARE) {
Tree* tree = ( WhichShape==YSHAPE ) ? &treeY : &treeDoubleY;
tree->Draw();
} else {
GLfloat blue[] = { 0.2f, 0.2f, 0.8f, 1.0f };
GLfloat green[] = { 0.3f, 0.6f, 0.3f, 1.0f };
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, green);
treeDoubleYDLS.Draw();
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, blue);
treeDoubleYSDLS.Draw();
}
if (EigenVectorsOn) {
if ( WhichMethod == SDLS || WhichMethod == DLS || WhichMethod == PURE_PSEUDO ) {
Jacobian* jacob;
switch ( WhichShape ) {
case YSHAPE:
jacob = jacobY;
break;
case DBLYSHAPE:
jacob = jacobDoubleY;
break;
default:
assert ( 0 );
}
jacob->DrawEigenVectors();
}
}
glFlush();
/* FOLLOWING BLOCK OF CODE USED FOR MAKING MOVIES
* if ( WhichMethod == JACOB_TRANS && WhichShape==DBLYSHAPE && (SleepCounter%SleepsPerStep)==0 ) {
* if (DumpCounter==0) {
* T = 0.0; // Set time back to zero
}
if ( DumpCounter >= DumpCounterStart && DumpCounter<DumpCounterEnd ) {
theScreenImage.LoadFromOpenglBuffer();
int fileNum = DumpCounter - DumpCounterStart;
char filename[23];
sprintf( filename, "JTRANSPOSE/temp%03d.bmp", fileNum );
theScreenImage.WriteBmpFile( filename );
}
DumpCounter++;
}
*/
glutSwapBuffers();
}
