KinematicChain.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2013 University of Jaume-I.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  * 
00008  * Contributors:
00009  *     Mario Prats
00010  *     Javier Perez
00011  */
00012 
00013 #include <uwsim/KinematicChain.h>
00014 #include <osgDB/Registry>
00015 #include <osgDB/ReadFile>
00016 
00017 KinematicChain::KinematicChain(int nlinks, int njoints)
00018 {
00019   jointType.resize(njoints);
00020   mimic.resize(njoints);
00021   limits.resize(njoints);
00022   names.resize(njoints);
00023   q.resize(njoints);
00024   qLastSafe.resize(njoints);
00025   memset(&(q.front()), 0, njoints * sizeof(double));
00026   started = 0;
00027 }
00028 
00029 void KinematicChain::setJointPosition(double *newq, int n)
00030 {
00031   int offset = 0;
00032   for (int i = 0; i < getNumberOfJoints(); i++)
00033   {
00034     if (i - offset >= n)
00035       break;
00036 
00037     if (jointType[i] == 0 || mimic[i].joint != i)
00038     {
00039       offset++;
00040       q[i] = 0;
00041     }
00042     else
00043     {
00044       if (newq[i - offset] < limits[i].first)
00045         q[i] = limits[i].first;
00046       else if (newq[i - offset] > limits[i].second)
00047         q[i] = limits[i].second;
00048       else
00049       {
00050         if (!isnan(q[i]))
00051           q[i] = newq[i - offset];
00052         else
00053         {
00054           std::cerr << "KinematicChain::setJointPosition received NaN" << std::endl;
00055           OSG_FATAL << "KinematicChain::setJointPosition received NaN" << std::endl;
00056         }
00057       }
00058     }
00059   }
00060   updateJoints (q);
00061 }
00062 
00063 void KinematicChain::setJointVelocity(double *qdot, int n)
00064 {
00065   //Time issues
00066   double elapsed = 0;
00067   if (started != 0)
00068   {
00069     ros::WallDuration t_diff = ros::WallTime::now() - last;
00070     elapsed = t_diff.toSec();
00071     //If elapsed>MAX_ELAPSED, consider this is sent by a different publisher, so that the counter has to restart
00072     if (elapsed > 1)
00073       elapsed = 0;
00074   }
00075 
00076   started = 1;
00077   last = ros::WallTime::now();
00078 
00079   int offset = 0;
00080   for (int i = 0; i < getNumberOfJoints(); i++)
00081   {
00082     if (i - offset >= n)
00083       break;
00084 
00085     if (jointType[i] == 0 || mimic[i].joint != i)
00086       offset++;
00087     else
00088     {
00089       if (q[i] + (qdot[i - offset] * elapsed) < limits[i].first)
00090         q[i] = limits[i].first;
00091       else if (q[i] + (qdot[i - offset] * elapsed) > limits[i].second)
00092         q[i] = limits[i].second;
00093       else
00094         q[i] += qdot[i - offset] * elapsed;
00095     }
00096   }
00097   updateJoints (q);
00098 }
00099 
00100 void KinematicChain::setJointPosition(std::vector<double> &q, std::vector<std::string> names)
00101 {
00102   if (names.size() > 0)
00103   {
00104     std::vector<double> newq;
00105     for (int i = 0; i < getNumberOfJoints(); i++)
00106     {
00107       if (not (jointType[i] == 0 || mimic[i].joint != i))
00108       { //Check it is not fixed nor mimic
00109         int found = 0;
00110         for (int j = 0; j < names.size() && !found; j++)
00111         {
00112           if (this->names[i] == names[j])
00113           {
00114             found = 1;
00115             newq.push_back(q[j]);
00116           }
00117         }
00118         if (!found)
00119           newq.push_back(this->q[i]);
00120       }
00121     }
00122     setJointPosition(&(newq.front()), newq.size());
00123   }
00124   else
00125     setJointPosition(&(q.front()), q.size());
00126 }
00127 
00128 void KinematicChain::setJointVelocity(std::vector<double> &qdot, std::vector<std::string> names)
00129 {
00130   if (names.size() > 0)
00131   {
00132     std::vector<double> newq;
00133     for (int i = 0; i < getNumberOfJoints(); i++)
00134     {
00135       if (not (jointType[i] == 0 || mimic[i].joint != i))
00136       { //Check it is not fixed nor mimic
00137         int found = 0;
00138         for (int j = 0; j < names.size() && !found; j++)
00139         {
00140           if (this->names[i] == names[j])
00141           {
00142             found = 1;
00143             newq.push_back(qdot[j]);
00144           }
00145         }
00146         if (!found)
00147           newq.push_back(0.0);
00148       }
00149     }
00150     setJointVelocity(&(newq.front()), newq.size());
00151   }
00152   else
00153     setJointVelocity(&(qdot.front()), qdot.size());
00154 }
00155 
00156 std::vector<double> KinematicChain::getJointPosition()
00157 {
00158   std::vector<double> validq;
00159   for (int i = 0; i < getNumberOfJoints(); i++)
00160   {
00161     if (jointType[i] != 0 && mimic[i].joint == i)
00162       validq.push_back(q[i]);
00163   }
00164   return validq;
00165 }
00166 
00167 std::vector<std::string> KinematicChain::getJointName()
00168 {
00169   std::vector<std::string> validq;
00170   for(int i=0;i<getNumberOfJoints();i++)
00171   {
00172     if(jointType[i]!=0 && mimic[i].joint==i) 
00173       validq.push_back(names[i]);
00174   }
00175   return validq;
00176 }
00177 
00178 std::map<std::string, double> KinematicChain::getFullJointMap()
00179 {
00180   std::map<std::string, double> map;
00181   for(int i=0;i<getNumberOfJoints();i++)
00182   {
00183     if (jointType[i] != 0)
00184       map[names[i]]=q[mimic[i].joint];
00185 
00186   }
00187 
00188   return map;
00189 }
00190 
00191 KinematicChain::~KinematicChain()
00192 {
00193 }


uwsim
Author(s): Mario Prats
autogenerated on Mon Oct 6 2014 08:24:07