JointPath.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00009 
00016 #include "JointPath.h"
00017 #include "Link.h"
00018 #include <hrpUtil/MatrixSolvers.h>
00019 #include <algorithm>
00020 
00021 using namespace std;
00022 using namespace hrp;
00023 
00024 
00025 JointPath::JointPath()
00026 {
00027     initialize();
00028 }
00029 
00030 
00031 JointPath::JointPath(Link* base, Link* end)
00032     : linkPath(base, end), 
00033       joints(linkPath.size())
00034 {
00035     initialize();
00036     extractJoints();
00037 }
00038 
00039 
00040 JointPath::JointPath(Link* end)
00041     : linkPath(end), 
00042       joints(linkPath.size())
00043 {
00044     initialize();
00045     extractJoints();
00046 }
00047 
00048 
00049 void JointPath::initialize()
00050 {
00051     maxIKErrorSqr = 1.0e-6 * 1.0e-6;
00052     isBestEffortIKMode = false;
00053 }
00054         
00055 
00056 JointPath::~JointPath()
00057 {
00058 
00059 }
00060 
00061 
00062 bool JointPath::find(Link* base, Link* end)
00063 {
00064     if(linkPath.find(base, end)){
00065         extractJoints();
00066     }
00067     onJointPathUpdated();
00068 
00069     return (!joints.empty());
00070 }
00071 
00072 
00073 bool JointPath::find(Link* end)
00074 {
00075     linkPath.find(end);
00076     extractJoints();
00077     onJointPathUpdated();
00078         
00079     return !joints.empty();
00080 }
00081 
00082 
00083 void JointPath::extractJoints()
00084 {
00085     numUpwardJointConnections = 0;
00086 
00087     int n = linkPath.size();
00088     if(n <= 1){
00089         joints.clear();
00090     } else {
00091         int i = 0;
00092         if(linkPath.isDownward(i)){
00093             i++;
00094         }
00095         joints.resize(n); // reserve size n buffer
00096         joints.clear();
00097         int m = n - 1;
00098         while(i < m){
00099             Link* link = linkPath[i];
00100             if(link->jointId >= 0){
00101                 if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){
00102                     joints.push_back(link);
00103                     if(!linkPath.isDownward(i)){
00104                         numUpwardJointConnections++;
00105                     }
00106                 }
00107             }
00108             ++i;
00109         }
00110         if(linkPath.isDownward(m-1)){
00111             Link* link = linkPath[m];
00112             if(link->jointId >= 0){
00113                 if(link->jointType == Link::ROTATIONAL_JOINT || link->jointType == Link::SLIDE_JOINT){
00114                     joints.push_back(link);
00115                 }
00116             }
00117         }
00118     }
00119 }
00120 
00121 
00122 void JointPath::onJointPathUpdated()
00123 {
00124 
00125 }
00126 
00127 
00128 void JointPath::calcJacobian(dmatrix& out_J) const
00129 {
00130     const int n = joints.size();
00131     out_J.resize(6, n);
00132         
00133     if(n > 0){
00134                 
00135         Link* targetLink = linkPath.endLink();
00136                 
00137         for(int i=0; i < n; ++i){
00138                         
00139             Link* link = joints[i];
00140                         
00141             switch(link->jointType){
00142                                 
00143             case Link::ROTATIONAL_JOINT:
00144             {
00145                 Vector3 omega(link->R * link->a);
00146                 Vector3 arm(targetLink->p - link->p);
00147                 if(!isJointDownward(i)){
00148                     omega *= -1.0;
00149                 } 
00150                 Vector3 dp(omega.cross(arm));
00151                 out_J.col(i) << dp, omega;
00152             }
00153             break;
00154                                 
00155             case Link::SLIDE_JOINT:
00156             {
00157                 Vector3 dp(link->R * link->d);
00158                 if(!isJointDownward(i)){
00159                     dp *= -1.0;
00160                 }
00161                 out_J.col(i) << dp, Vector3::Zero();
00162             }
00163             break;
00164                                 
00165             default:
00166                 out_J.col(i).setZero();
00167             }
00168         }
00169     }
00170 }
00171 
00172 
00173 void JointPath::setMaxIKError(double e)
00174 {
00175     maxIKErrorSqr = e * e;
00176 }
00177 
00178 
00179 void JointPath::setBestEffortIKMode(bool on)
00180 {
00181     isBestEffortIKMode = on;
00182 }
00183 
00184 
00185 bool JointPath::calcInverseKinematics
00186 (const Vector3& base_p, const Matrix33& base_R, const Vector3& end_p, const Matrix33& end_R)
00187 {
00188     Link* baseLink = linkPath.baseLink();
00189     baseLink->p = base_p;
00190     baseLink->R = base_R;
00191 
00192     if(!hasAnalyticalIK()){
00193         calcForwardKinematics();
00194     }
00195         
00196     return calcInverseKinematics(end_p, end_R);
00197 }
00198 
00199 
00200 bool JointPath::calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R)
00201 {
00202     static const int MAX_IK_ITERATION = 50;
00203     static const double LAMBDA = 0.9;
00204     
00205     if(joints.empty()){
00206         if(linkPath.empty()){
00207             return false;
00208         }
00209         if(baseLink() == endLink()){
00210             baseLink()->p = end_p;
00211             baseLink()->R = end_R;
00212             return true;
00213         } else {
00214             // \todo implement here
00215             return false;
00216         }
00217     }
00218     
00219     const int n = numJoints();
00220 
00221     Link* target = linkPath.endLink();
00222 
00223     std::vector<double> qorg(n);
00224     for(int i=0; i < n; ++i){
00225         qorg[i] = joints[i]->q;
00226     }
00227 
00228     dmatrix J(6, n);
00229     dvector dq(n);
00230     dvector v(6);
00231 
00232     double errsqr = maxIKErrorSqr * 100.0;
00233     bool converged = false;
00234 
00235     for(int i=0; i < MAX_IK_ITERATION; i++){
00236         
00237         calcJacobian(J);
00238         
00239         Vector3 dp(end_p - target->p);
00240         Vector3 omega(target->R * omegaFromRot(target->R.transpose() * end_R));
00241 
00242         if(isBestEffortIKMode){
00243             const double errsqr0 = errsqr;
00244             errsqr = dp.dot(dp) + omega.dot(omega);
00245             if(fabs(errsqr - errsqr0) < maxIKErrorSqr){
00246                 converged = true;
00247                 break;
00248             }
00249         } else {
00250             const double errsqr = dp.dot(dp) + omega.dot(omega);
00251             if(errsqr < maxIKErrorSqr){
00252                 converged = true;
00253                 break;
00254             }
00255         }
00256 
00257         v << dp, omega;
00258                 
00259         if(n == 6){ 
00260             solveLinearEquationLU(J, v, dq);
00261         } else {
00262             solveLinearEquationSVD(J, v, dq);  // dq = pseudoInverse(J) * v
00263         }
00264                 
00265         for(int j=0; j < n; ++j){
00266             joints[j]->q += LAMBDA * dq(j);
00267         }
00268 
00269         calcForwardKinematics();
00270     }
00271 
00272     if(!converged){
00273         for(int i=0; i < n; ++i){
00274             joints[i]->q = qorg[i];
00275         }
00276         calcForwardKinematics();
00277     }
00278     
00279     return converged;
00280 }
00281 
00282 
00283 bool JointPath::hasAnalyticalIK()
00284 {
00285     return false;
00286 }
00287 
00288 
00289 std::ostream& operator<<(std::ostream& os, JointPath& path)
00290 {
00291     int n = path.numJoints();
00292     for(int i=0; i < n; ++i){
00293         Link* link = path.joint(i);
00294         os << link->name;
00295         if(i != n){
00296             os << (path.isJointDownward(i) ? " => " : " <= ");
00297         }
00298     }
00299     os << std::endl;
00300     return os;
00301 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55