00001
00007 #include "robodyn_controllers/Robot.h"
00008
00009 using namespace KDL;
00010 using namespace std;
00011
00012
00018 Robot::Robot()
00019 {
00020 }
00021
00022
00027 Robot::~Robot()
00028 {
00029 JointDataMap::iterator ittr;
00030
00031 for (ittr = jointDataMap.begin(); ittr != jointDataMap.end(); ittr++)
00032 {
00033 delete ittr->second;
00034 }
00035
00036 jointDataMap.clear();
00037
00038 JointTraitsMap::iterator ittrt;
00039
00040 for (ittrt = jointTraitsMap.begin(); ittrt != jointTraitsMap.end(); ittrt++)
00041 {
00042 delete ittrt->second;
00043 }
00044
00045 jointTraitsMap.clear();
00046 }
00047
00048
00057 KDL::Chain Robot::getChain(const std::string& fromSegment,
00058 const std::string& toSegment, KDL::Chain& chain)
00059 {
00060 if (!robotTree.getChain(fromSegment, toSegment, chain))
00061 {
00062 stringstream err;
00063 err << "Robot::getChain unable to find chain from " << fromSegment << " to " << toSegment;
00064 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00065 throw runtime_error(err.str());
00066 }
00067
00068 return chain;
00069 }
00070
00071
00080 KDL::Chain Robot::getChainToRoot(const std::string& fromSegment, KDL::Chain& chain)
00081 {
00082 return (getChain(fromSegment, robotTree.getRootSegment()->first, chain));
00083 }
00084
00085
00094 KDL::Chain Robot::getChainFromRoot(const std::string& toSegment, KDL::Chain& chain)
00095 {
00096 return (getChain(robotTree.getRootSegment()->first, toSegment, chain));
00097 }
00098
00099
00108 KDL::Chain Robot::getChainInReverse(const KDL::Chain& inChain, KDL::Chain& retChain)
00109 {
00110 Joint currentJoint;
00111
00112 for (int i = inChain.getNrOfSegments() - 1; i >= 0; i--)
00113 {
00114 if ((unsigned)i == inChain.getNrOfSegments() - 1)
00115 {
00116 currentJoint = Joint(Joint::None);
00117 }
00118 else
00119 {
00120 currentJoint = inChain.getSegment(i + 1).getJoint();
00121 }
00122
00123 retChain.addSegment(Segment(inChain.getSegment(i).getName(),
00124 currentJoint, inChain.getSegment(i).getFrameToTip().Inverse()));
00125 }
00126
00127 return retChain;
00128 }
00129
00130
00137 double Robot::getMass()
00138 {
00139 double m = 0.0;
00140 SegmentMap segmentMap = robotTree.getSegments();
00141
00142 SegmentMap::const_iterator end = segmentMap.end();
00143
00144 for (SegmentMap::const_iterator it = segmentMap.begin(); it != end; ++it)
00145 {
00146 m = m + (it->second).segment.getInertia().getMass();
00147 }
00148
00149 return m;
00150 }
00151
00152
00158 Vector Robot::getCenterOfMass()
00159 {
00160 Vector v;
00161
00162 Eigen::Matrix<double, 3, Eigen::Dynamic> locationMatrix(3, robotTree.getSegments().size() - 1);
00163 Eigen::VectorXd weightVector(robotTree.getSegments().size() - 1);
00164 Eigen::Vector3d temp;
00165
00166 unsigned i = 0;
00167 Frame parentFrame;
00168 Vector locationInBase;
00169 SegmentMap segmentMap = robotTree.getSegments();
00170
00171 std::string rootSegName = robotTree.getRootSegment()->first;
00172
00173 SegmentMap::const_iterator end = segmentMap.end();
00174
00175 for (SegmentMap::const_iterator it = segmentMap.begin(); it != end; ++it)
00176 {
00177 if (it->second.segment.getName().compare(rootSegName) != 0)
00178 {
00180 Frame parentFrame = getChainTransformTipToBase(it->second.parent->second.segment.getName()) *
00181 Frame(it->second.segment.getFrameToTip().M.RotZ(getJointPosition(it->second.segment.getName())));
00182
00183 locationInBase = parentFrame * it->second.segment.getInertia().getCOG();
00184
00185 locationMatrix(0, i) = locationInBase.x();
00186 locationMatrix(1, i) = locationInBase.y();
00187 locationMatrix(2, i) = locationInBase.z();
00188 weightVector(i) = it->second.segment.getInertia().getMass();
00189 i++;
00190 }
00191 }
00192
00193 temp = (locationMatrix * weightVector) * (1 / getMass());
00194
00195 v.x(temp(0));
00196 v.y(temp(1));
00197 v.z(temp(2));
00198
00199 return v;
00200 }
00201
00202
00218 Robot::JointTraits* Robot::addJoint(const std::string& jointName)
00219 {
00220 JointData* data = new JointData();
00221 data->curPosition = 0.0;
00222 data->curVelocity = 0.0;
00223 data->curTorque = 0.0;
00224
00225 jointDataMap.insert(std::pair<std::string, JointData*>(jointName, data));
00226
00227 JointTraits* traits = new JointTraits();
00228 traits->name = jointName;
00229 traits->maxPosition = 0.0;
00230 traits->minPosition = 0.0;
00231 traits->maxVelocity = 0.0;
00232 traits->maxTorque = 0.0;
00233 traits->springConstant = 0.0;
00234
00235 jointTraitsMap.insert(std::pair<std::string, JointTraits*>(jointName, traits));
00236
00237 return traits;
00238 }
00239
00240
00251 Robot::JointData* Robot::getJointData(const std::string& jointName)
00252 {
00253 JointDataMap::iterator it = jointDataMap.find(jointName);
00254
00255 if (it == jointDataMap.end())
00256 {
00257 stringstream err;
00258 err << "Robot::getJointData unknown joint " << jointName;
00259 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00260 throw runtime_error(err.str());
00261 }
00262
00263 return (it->second);
00264 }
00265
00266
00277 Robot::JointTraits* Robot::getJointTraits(const std::string& segmentName)
00278 {
00279 JointTraitsMap::iterator it = jointTraitsMap.find(segmentName);
00280
00281 if (it == jointTraitsMap.end())
00282 {
00283 stringstream err;
00284 err << "Robot::getJointTraits unknown joint " << segmentName;
00285 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00286 throw runtime_error(err.str());
00287 }
00288
00289 return (it->second);
00290 }
00291
00292
00303 Robot::JointTraitsVector Robot::getJointTraits()
00304 {
00305 JointTraitsVector traits;
00306 traits.clear();
00307
00308 JointTraitsMap::iterator it = jointTraitsMap.begin();
00309
00310 for (it = jointTraitsMap.begin(); it != jointTraitsMap.end(); it++)
00311 {
00312 traits.push_back(*(it->second));
00313 }
00314
00315 return (traits);
00316 }
00317
00318
00323 void Robot::zeroJoints()
00324 {
00325 JointDataMap::iterator ittr;
00326
00327 for (ittr = jointDataMap.begin(); ittr != jointDataMap.end(); ittr++)
00328 {
00329 ittr->second->curPosition = 0.0;
00330 ittr->second->curVelocity = 0.0;
00331 ittr->second->curTorque = 0.0;
00332 }
00333 }
00334
00335
00346 void Robot::setJointLimits(const std::string& jointName, double min, double max)
00347 {
00348 JointTraitsMap::iterator it = jointTraitsMap.find(jointName);
00349
00350 if (it == jointTraitsMap.end())
00351 {
00352 stringstream err;
00353 err << "Robot::setJointLimits unknown joint " << jointName;
00354 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00355 throw runtime_error(err.str());
00356 }
00357
00358 it->second->minPosition = min;
00359 it->second->maxPosition = max;
00360 }
00361
00362
00372 void Robot::setJointPosition(const std::string& jointName, double position)
00373 {
00374 JointDataMap::iterator it = jointDataMap.find(jointName);
00375
00376 if (it == jointDataMap.end())
00377 {
00378 stringstream err;
00379 err << "Robot::setJointPosition unknown joint " << jointName;
00380 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00381 throw runtime_error(err.str());
00382 }
00383
00384 it->second->curPosition = position;
00385 }
00386
00387
00396 double Robot::getJointPosition(const std::string& jointName)
00397 {
00398 JointDataMap::iterator it = jointDataMap.find(jointName);
00399
00400 if (it == jointDataMap.end())
00401 {
00402 stringstream err;
00403 err << "Robot::getJointPosition unknown joint " << jointName;
00404 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00405 throw runtime_error(err.str());
00406 }
00407
00408 return it->second->curPosition;
00409 }
00410
00411
00412
00422 void Robot::setJointVelocity(const std::string& jointName, double velocity)
00423 {
00424 JointDataMap::iterator it = jointDataMap.find(jointName);
00425
00426 if (it == jointDataMap.end())
00427 {
00428 stringstream err;
00429 err << "Robot::setJointVelocity unknown joint " << jointName;
00430 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00431 throw runtime_error(err.str());
00432 }
00433
00434 it->second->curVelocity = velocity;
00435 }
00436
00437
00446 double Robot::getJointVelocity(const std::string& jointName)
00447 {
00448 JointDataMap::iterator it = jointDataMap.find(jointName);
00449
00450 if (it == jointDataMap.end())
00451 {
00452 stringstream err;
00453 err << "Robot::getJointVelocity unknown joint " << jointName;
00454 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00455 throw runtime_error(err.str());
00456 }
00457
00458 return it->second->curVelocity;
00459 }
00460
00461
00471 void Robot::setJointTorque(const std::string& jointName, double torque)
00472 {
00473 JointDataMap::iterator it = jointDataMap.find(jointName);
00474
00475 if (it == jointDataMap.end())
00476 {
00477 stringstream err;
00478 err << "Robot::setJointTorque unknown joint " << jointName;
00479 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00480 throw runtime_error(err.str());
00481 }
00482
00483 it->second->curTorque = torque;
00484 }
00485
00486
00495 double Robot::getJointTorque(const std::string& jointName)
00496 {
00497 JointDataMap::iterator it = jointDataMap.find(jointName);
00498
00499 if (it == jointDataMap.end())
00500 {
00501 stringstream err;
00502 err << "Robot::getJointTorque unknown joint " << jointName;
00503 NasaCommonLogging::Logger::log("gov.nasa.controllers.Robot", log4cpp::Priority::ERROR, err.str());
00504 throw runtime_error(err.str());
00505 }
00506
00507 return it->second->curTorque;
00508 }
00509
00510
00515 JntArray Robot::getChainJointPositions(const KDL::Chain& chain)
00516 {
00517 JntArray jointArray;
00518 unsigned i = 0;
00519 unsigned j = 0;
00520
00521 jointArray.resize(chain.getNrOfJoints());
00522
00523 for (i = 0; i < chain.getNrOfSegments(); i++)
00524 {
00525 if (chain.getSegment(i).getJoint().getType() != Joint::None)
00526 {
00527 jointArray(j++)
00528 = jointDataMap[chain.getSegment(i).getName()]->curPosition;
00529 }
00530 }
00531
00532 return jointArray;
00533 }
00534
00535
00540 JntArray Robot::getChainJointVelocities(const KDL::Chain& chain)
00541 {
00542 JntArray jointArray;
00543 unsigned i = 0;
00544 unsigned j = 0;
00545
00546 jointArray.resize(chain.getNrOfJoints());
00547
00548 for (i = 0; i < chain.getNrOfSegments(); i++)
00549 {
00550 if (chain.getSegment(i).getJoint().getType() != Joint::None)
00551 {
00552 jointArray(j++)
00553 = jointDataMap[chain.getSegment(i).getName()]->curVelocity;
00554 }
00555 }
00556
00557 return jointArray;
00558 }
00559
00560
00575 void Robot::getChainJacobian(const KDL::Chain& chain,
00576 const KDL::JntArray& positions, KDL::Jacobian& jacobian)
00577 {
00578 if (chain.getNrOfJoints() > 0)
00579 {
00580 jacobian.resize(chain.getNrOfJoints());
00581
00582 ChainJntToJacSolver jacSolver(chain);
00583 jacSolver.JntToJac(positions, jacobian);
00584 }
00585 }
00586
00587
00593 KDL::Frame Robot::getChainTransform(const KDL::Chain& chain)
00594 {
00595 return getChainTransform(chain, getChainJointPositions(chain));
00596 }
00597
00598
00604 Frame Robot::getChainTransformBaseToTip(const std::string& segmentName)
00605 {
00606 KDL::Chain chain;
00607
00608 getChainFromRoot(segmentName, chain);
00609 return getChainTransform(chain);
00610 }
00611
00612
00618 Frame Robot::getChainTransformTipToBase(const std::string& segmentName)
00619 {
00620 KDL::Chain chain;
00621
00622 getChainToRoot(segmentName, chain);
00623 return getChainTransform(chain);
00624 }
00625
00626
00632 KDL::Frame Robot::getChainTransform(const KDL::Chain& chain, const KDL::JntArray& pos)
00633 {
00634 KDL::Frame retVal;
00635
00636 KDL::ChainFkSolverPos_recursive fkPosSolver(chain);
00637 fkPosSolver.JntToCart(pos, retVal, -1);
00638
00639 return retVal;
00640 }
00641
00642
00647 Frame Robot::getStaticTransform(const std::string& transformName)
00648 {
00649 return staticTransforms[transformName];
00650 }
00651
00652
00657 void Robot::setStaticTransform(const std::string& transformName, const Frame& transform)
00658 {
00659 staticTransforms[transformName] = transform;
00660 }
00661
00662
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682
00683
00684
00685
00693 void Robot::printChain(const Chain& chain)
00694 {
00695 printf("%12s %12s %12s \n", "joint", "type", "link");
00696 printf("============ ============ ============\n");
00697
00698 for (size_t i = 0; i < chain.getNrOfSegments(); i++)
00699 {
00700 printf("%s", Robot::segmentToString(chain.segments[i]).c_str());
00701 }
00702
00703 printf("--------------------------------------\n\n");
00704 }
00705
00706
00716 std::string Robot::segmentToString(const Segment& segment)
00717 {
00718 std::stringstream ss;
00719
00720 if (segment.getJoint().getType() != Joint::None)
00721 {
00722 ss.width(12);
00723 ss << segment.getJoint().getName();
00724 ss.width(13);
00725 ss << segment.getJoint().getTypeName().c_str();
00726 ss.width(14);
00727 ss << "";
00728 ss << frameToString(segment.getFrameToTip()) << std::endl;
00729 }
00730 else
00731 {
00732 ss.width(39);
00733 ss << "";
00734 ss << frameToString(segment.getFrameToTip()) << std::endl;
00735
00736 }
00737
00738 ss.width(26);
00739 ss << " ";
00740 ss.width(12);
00741 ss << segment.getName() << " " << rigidBodyInertiaToString(segment.getInertia()) << std::endl;
00742
00743 return ss.str();
00744 }
00745
00746
00756 std::string Robot::rigidBodyInertiaToString(const KDL::RigidBodyInertia& rbi)
00757 {
00758 std::stringstream ss;
00759
00760 ss.setf(ios::fixed, ios::floatfield);
00761 ss.precision(3);
00762
00763 ss << "m=";
00764 ss.width(8);
00765 ss << rbi.getMass();
00766 ss << " cog=" << vectorToString(rbi.getCOG());
00767
00768 return ss.str();
00769 }
00770
00771
00781 std::string Robot::frameToString(const Frame& frame)
00782 {
00783 std::stringstream ss;
00784
00785 ss << "rpy=" << rotationToRpyString(frame.M) << " xyz=" << vectorToString(frame.p);
00786
00787 return ss.str();
00788 }
00789
00790
00799 std::string Robot::rotationToRpyString(const Rotation& rotation)
00800 {
00801 std::stringstream ss;
00802
00803 ss.setf(ios::fixed, ios::floatfield);
00804 ss.precision(3);
00805
00806 double r, p, y;
00807 rotation.GetRPY(r, p, y);
00808
00809 ss << "[ ";
00810 ss.width(8);
00811 ss << r << ", ";
00812 ss.width(8);
00813 ss << p << ", ";
00814 ss.width(8);
00815 ss << y << " ] ";
00816
00817 return ss.str();
00818 }
00819
00820
00829 std::string Robot::rotationToString(const Rotation& rotation)
00830 {
00831 std::stringstream ss;
00832
00833 ss.setf(ios::fixed, ios::floatfield);
00834 ss.precision(3);
00835
00836 ss << "[ ";
00837 ss.width(8);
00838 ss << rotation.data[0] << ", ";
00839 ss.width(8);
00840 ss << rotation.data[1] << ", ";
00841 ss.width(8);
00842 ss << rotation.data[2] << " ] ";
00843
00844 ss << "[ ";
00845 ss.width(8);
00846 ss << rotation.data[3] << ", ";
00847 ss.width(8);
00848 ss << rotation.data[4] << ", ";
00849 ss.width(8);
00850 ss << rotation.data[5] << " ] ";
00851
00852 ss << "[ ";
00853 ss.width(8);
00854 ss << rotation.data[6] << ", ";
00855 ss.width(8);
00856 ss << rotation.data[7] << ", ";
00857 ss.width(8);
00858 ss << rotation.data[8] << " ] ";
00859
00860 return ss.str();
00861 }
00862
00863
00872 std::string Robot::vectorToString(const Vector& vector)
00873 {
00874 std::stringstream ss;
00875
00876 ss.setf(ios::fixed, ios::floatfield);
00877 ss.precision(3);
00878
00879 ss << "[ ";
00880 ss.width(8);
00881 ss << vector.data[0] << ", ";
00882 ss.width(8);
00883 ss << vector.data[1] << ", ";
00884 ss.width(8);
00885 ss << vector.data[2] << " ]";
00886
00887 return ss.str();
00888 }
00889
00890
00899 std::string Robot::jacobianToString(KDL::Jacobian& jacobian)
00900 {
00901 std::stringstream ss;
00902
00903 ss.setf(ios::fixed, ios::floatfield);
00904 ss.precision(3);
00905
00906 unsigned int rows = jacobian.rows();
00907 unsigned int cols = jacobian.columns();
00908
00909 ss << "[\n";
00910
00911 for (unsigned int i = 0 ; i < rows; i++)
00912 {
00913 ss << " [";
00914
00915 for (unsigned int j = 0; j < cols - 1; j++)
00916 {
00917 ss.width(8);
00918 ss << jacobian(i, j) << ", ";
00919 }
00920
00921 ss.width(8);
00922 ss << jacobian(i, cols - 1) << " ]\n";
00923 }
00924
00925 ss << "]";
00926
00927 return ss.str();
00928 }