Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

Kinematics.cc

Go to the documentation of this file.
00001 //This class is ported from Carnegie Mellon's 2001 Robosoccer entry, and falls under their license:
00002 /*=========================================================================
00003     CMPack'02 Source Code Release for OPEN-R SDK v1.0
00004     Copyright (C) 2002 Multirobot Lab [Project Head: Manuela Veloso]
00005     School of Computer Science, Carnegie Mellon University
00006   -------------------------------------------------------------------------
00007     This software is distributed under the GNU General Public License,
00008     version 2.  If you do not have a copy of this licence, visit
00009     www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
00010     Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
00011     in the hope that it will be useful, but WITHOUT ANY WARRANTY,
00012     including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00013   -------------------------------------------------------------------------
00014     Additionally licensed to Sony Corporation under the following terms:
00015 
00016     This software is provided by the copyright holders AS IS and any
00017     express or implied warranties, including, but not limited to, the
00018     implied warranties of merchantability and fitness for a particular
00019     purpose are disclaimed.  In no event shall authors be liable for
00020     any direct, indirect, incidental, special, exemplary, or consequential
00021     damages (including, but not limited to, procurement of substitute
00022     goods or services; loss of use, data, or profits; or business
00023     interruption) however caused and on any theory of liability, whether
00024     in contract, strict liability, or tort (including negligence or
00025     otherwise) arising in any way out of the use of this software, even if
00026     advised of the possibility of such damage.
00027   =========================================================================
00028 */
00029 
00030 #ifdef PLATFORM_LINUX
00031 #include <stdlib.h>
00032 #include <stdio.h>
00033 #endif
00034 
00035 #include <math.h>
00036 
00037 #include "Kinematics.h"
00038 
00039 // #define DEBUG
00040 
00041 // ERS-210 Leg Parameters
00042 const vector3d f_body_to_shoulder       ( 59.50, 59.20,   0.00);
00043 const vector3d f_leg_shoulder_to_knee   ( 12.80,  0.50, -64.00);
00044 const vector3d f_leg_knee_to_ball       (-18.00,  0.00, -67.23);
00045 
00046 const vector3d h_body_to_shoulder       (-59.50, 59.20,   0.00);
00047 const vector3d h_leg_shoulder_to_knee   (-12.80,  0.50, -64.00);
00048 const vector3d h_leg_knee_to_ball       ( 18.00,  0.00, -74.87);
00049 
00050 /*
00051   18 : measured
00052   67.23 = sqrt(69.6^2 - 18^2)
00053   74.87 = sqrt(77^2 - 18^2)
00054   0.2616 = asin(18 / 69.6)
00055   0.2316 = asin(18 / 77)
00056 */
00057 
00058 /* ERS-110
00059 const vector3d f_body_to_shoulder       ( 44.85, 26.50,   0.00);
00060 const vector3d f_leg_shoulder_to_knee   ( 13.00,  5.50, -61.00);
00061 const vector3d f_leg_knee_to_ball       ( -9.50,  1.06, -58.00); // ??
00062 
00063 const vector3d h_body_to_shoulder       (-44.85, 26.50,   0.00);
00064 const vector3d h_leg_shoulder_to_knee   (-13.00,  5.50, -61.00);
00065 const vector3d h_leg_knee_to_ball       ( 19.00,  1.06, -69.00); // ??
00066 */
00067 
00068 #ifdef PLATFORM_LINUX
00069 void print(double *angles,vector3d pos)
00070 {
00071   printf("A(%7.4f,%7.4f,%7.4f) P(%7.2f,%7.2f,%7.2f)\n",
00072    angles[0],angles[1],angles[2],
00073    pos.x,pos.y,pos.z);
00074 }
00075 #endif
00076 
00077 
00078 const vector3d f_upper = f_leg_shoulder_to_knee;
00079 const vector3d f_lower = f_leg_knee_to_ball;
00080 const vector3d h_upper = h_leg_shoulder_to_knee;
00081 const vector3d h_lower = h_leg_knee_to_ball;
00082 
00083 //==== Globals ====//
00084 #ifdef PLATFORM_LINUX
00085 int g_leg;
00086 vector3d g_target;
00087 #endif
00088 
00089 static int errors;
00090 
00091 void KinClearErrors()
00092 {
00093   errors = 0;
00094 }
00095 
00096 int KinGetErrors()
00097 {
00098   return(errors);
00099 }
00100 
00101 double GetTrigAngle(double a,double b,double d,double mn,double mx,bool add)
00102 // Analytic solution to a*sin(x) + b*cos(x) = d
00103 {
00104   double theta;
00105   double t,f,c;
00106   int err;
00107 
00108   f = d / sqrt(a*a + b*b);
00109   err = 0;
00110 
00111   if(fabs(f) > 1.0){
00112 #ifdef PLATFORM_LINUX
00113     /*
00114     printf("Out of range (distance=%g) leg=%d target=(%g,%g,%g)\n",f,
00115            g_leg,g_target.x,g_target.y,g_target.z);
00116     */
00117 #endif
00118     f = (f > 0.0)? 1.0 : -1.0;
00119     err = 1;
00120   }
00121 
00122   t = atan2(a,b);
00123   c = acos(f);
00124 
00125   theta = add? (t + c) : (t - c);
00126 
00127   if(theta < mn){
00128 #ifdef PLATFORM_LINUX
00129     /*
00130     printf("Out of range (angle to small) leg=%d target=(%g,%g,%g)\n",
00131            g_leg,g_target.x,g_target.y,g_target.z);
00132     */
00133 #endif
00134     errors++;
00135     return(mn);
00136   }else if(theta > mx){
00137 #ifdef PLATFORM_LINUX
00138     /*
00139     printf("Out of range (angle to large) leg=%d target=(%g,%g,%g)\n",
00140            g_leg,g_target.x,g_target.y,g_target.z);
00141     */
00142 #endif
00143     errors++;
00144     return(mx);
00145   }else{
00146     errors += err;
00147     return(theta);
00148   }
00149 }
00150 
00151 /*
00152 Angle Limits:
00153            ===Software====  ==Mechanical===
00154   Rotator  [-117.0, 117.0]  [-120.0, 120.0]
00155   Shoulder [ -11.0,  97.0]  [ -14.0, 100.0]
00156   Knee     [ -27.0, 147.0]  [ -30.0, 150.0]
00157 */
00158 
00159 const double rotator_min  = RAD(-117.0);
00160 const double rotator_max  = RAD( 117.0);
00161 const double shoulder_min = RAD( -11.0);
00162 const double shoulder_max = RAD(  97.0);
00163 const double knee_max     = RAD( 147.0);
00164 const double knee_min     = RAD( -27.0);
00165 
00166 const double rotator_kmin  = RAD(-90.0);
00167 const double rotator_kmax  = RAD( 90.0);
00168 const double shoulder_kmin = shoulder_min;
00169 const double shoulder_kmax = RAD( 90.0);
00170 const double knee_kmax     = knee_max;
00171 const double f_knee_kmin   = 0.2616;
00172 const double h_knee_kmin   = 0.2316;
00173 
00174 const double tail_min = RAD(-22);
00175 const double tail_max = RAD( 22);
00176 
00177 const double head_tilt_min = RAD(-88.5);
00178 const double head_tilt_max = RAD( 43.0);
00179 const double head_pan_min  = RAD(-89.6);
00180 const double head_pan_max  = RAD( 89.6);
00181 const double head_roll_min = RAD(-29.0);
00182 const double head_roll_max = RAD( 29.0);
00183 
00184 
00185 void GetLegAngles(double *angles,vector3d target,int leg)
00186 {
00187   vector3d targ,pos;
00188   double knee,shoulder,rotator;
00189   double a,b,d,dist;
00190   bool front = leg < 2;
00191 
00192 #ifdef PLATFORM_LINUX
00193   g_leg    = leg;
00194   g_target = target;
00195   // printf("GLA: target=(%g,%g,%g)\n",target.x,target.y,target.z);
00196 #endif
00197 
00198   knee = shoulder = rotator = 0.0;
00199 
00200   if(leg % 2) target.y = -target.y;
00201 
00202   if(front){
00203     targ = target - f_body_to_shoulder;
00204     dist = targ.sqlength();
00205 
00206     // Calculate knee angle
00207     a = -2*(f_upper.x*f_lower.z - f_upper.z*f_lower.x);
00208     b =  2*(f_upper.x*f_lower.x + f_upper.z*f_lower.z);
00209     d = (dist - f_upper.sqlength() - f_lower.sqlength() - 2*f_upper.y*f_lower.y);
00210     knee = GetTrigAngle(a,b,d,f_knee_kmin,knee_kmax,true);
00211 
00212     // Calculate shoulder angle
00213     pos = f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-knee);
00214     shoulder = GetTrigAngle(-pos.z,pos.y,targ.y,shoulder_kmin,shoulder_kmax,false);
00215 
00216     // Calculate rotator angle
00217     // pos = pos.rotate_x(shoulder);
00218     pos.z = sin(shoulder)*pos.y + cos(shoulder)*pos.z;
00219     rotator = GetTrigAngle(-pos.z,pos.x,targ.x,rotator_min,rotator_max,target.z > 0.0);
00220 
00221 #ifdef DEBUG
00222     // Test
00223     //    pos = (f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-knee))
00224     //           .rotate_x(shoulder).rotate_y(-rotator);
00225     //    printf("D(%f,%f,%f)\n",targ.x - pos.x,targ.y - pos.y,targ.z - pos.z);
00226 #endif
00227   }else{
00228     targ = target - h_body_to_shoulder;
00229     dist = targ.sqlength();
00230 
00231     // Calculate knee angle
00232     a = 2*(h_upper.x*h_lower.z - h_upper.z*h_lower.x);
00233     b = 2*(h_upper.x*h_lower.x + h_upper.z*h_lower.z);
00234     d = (dist - h_upper.sqlength() - h_lower.sqlength() - 2*h_upper.y*h_lower.y);
00235     knee = GetTrigAngle(a,b,d,h_knee_kmin,knee_kmax,true);
00236 
00237     // Calculate shoulder angle
00238     pos = h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y(knee);
00239     shoulder = GetTrigAngle(-pos.z,pos.y,targ.y,shoulder_kmin,shoulder_kmax,false);
00240 
00241     // Calculate rotator angle
00242     pos.z = sin(shoulder)*pos.y + cos(shoulder)*pos.z;
00243     rotator = GetTrigAngle(-pos.z,-pos.x,-targ.x,rotator_min,rotator_max,target.z > 0.0);
00244 
00245     /*
00246     if(fabs(theta - rotator) > 0.01){
00247       printf("ERROR(%f - %f = %f)\n",theta,rotator,theta - rotator);
00248     }
00249     */
00250 
00251 #ifdef DEBUG
00252     // Test
00253     //    pos = (h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y(angles[2]))
00254     //           .rotate_x(angles[1]).rotate_y(angles[0]);
00255     //    printf("D(%f,%f,%f)\n",targ.x - pos.x,targ.y - pos.y,targ.z - pos.z);
00256 #endif
00257   }
00258 
00259   angles[0] = rotator;
00260   angles[1] = shoulder;
00261   angles[2] = knee;
00262 }
00263 
00264 void GetLegAngles(double *angles,vector3d target[4],
00265                   double body_angle,double body_height)
00266 {
00267   vector3d p;
00268   int i;
00269 
00270   for(i=0; i<4; i++){
00271     p = target[i];
00272     p.z -= body_height;
00273     p = p.rotate_y(-body_angle);
00274     GetLegAngles(angles + 3*i,p,i);
00275   }
00276 }
00277 
00278 void GetLegAngles(double *angles,vector3d target[4],BodyPosition &bp)
00279 {
00280   vector3d p;
00281   int i;
00282 
00283   for(i=0; i<4; i++){
00284     p = (target[i] - bp.loc).rotate_z(bp.angle.z).rotate_y(-bp.angle.y);
00285     GetLegAngles(angles + 3*i,p,i);
00286   }
00287 }
00288 
00289 void GetLegAngles(double *angles,vector3d target,BodyPosition &bp,int leg)
00290 {
00291   vector3d p;
00292 
00293   p = (target - bp.loc).rotate_z(bp.angle.z).rotate_y(-bp.angle.y);
00294   GetLegAngles(angles,p,leg);
00295 }
00296 
00297 
00298 void GetLegPosition(vector3d& p, const double* ang, int leg)
00299 {
00300   if(leg < 2){
00301     p = f_body_to_shoulder +
00302       (f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-ang[2]))
00303       .rotate_x(ang[1]).rotate_y(-ang[0]);
00304   }else{
00305     p = h_body_to_shoulder +
00306       (h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y( ang[2]))
00307       .rotate_x(ang[1]).rotate_y( ang[0]);
00308   }
00309   
00310   if(leg % 2)
00311     p.y = -p.y;
00312 }
00313 
00314 
00315 void GetBodyLocation(vector3d &ball,vector3d &toe,const double *ang,int leg)
00316 // project various parts of foot that could touch by given joint angles
00317 {
00318   if(leg < 2){
00319     ball = f_body_to_shoulder +
00320            (f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-ang[2]))
00321            .rotate_x(ang[1]).rotate_y(-ang[0]);
00322   }else{
00323     ball = h_body_to_shoulder +
00324            (h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y( ang[2]))
00325            .rotate_x(ang[1]).rotate_y( ang[0]);
00326   }
00327 
00328   // TODO: toes
00329   toe = ball;
00330 
00331   if(leg % 2){
00332     ball.y = -ball.y;
00333     toe.y = -toe.y;
00334   }
00335 }
00336 
00337 void GetHeadAngles(double *angles,vector3d target,
00338                    double body_angle,double body_height)
00339 {
00340   double tilt,pan;
00341 
00342   vector3d neck;
00343   double height;
00344 
00345 #ifdef PLATFORM_APERIOS
00346   //pprintf(TextOutputStream,"target (%g,%g,%g) body_angle %g body_height %g\n",
00347   //        target.x,target.y,target.z,
00348   //        body_angle,body_height);
00349 #endif
00350 
00351   // translate target so it is relative to base of neck
00352   neck = body_to_neck.rotate_y(body_angle);
00353   height = body_height + neck.z;
00354   target.z -= height;
00355   
00356   double target_xz_dist; // distance in robot's xz plane of target
00357 
00358   target_xz_dist=hypot(target.x,target.z);
00359 
00360   // assumes that camera is aligned with base of neck
00361   // can only see if not too close to neck
00362   if(target_xz_dist > neck_to_camera.z && target.length() > neck_to_camera.length()) {
00363     double angle_base_target; // angle from base of neck to target xz
00364     double angle_base_target_camera; // angle between base and camera from target xz
00365     
00366     angle_base_target = atan2(target.z,target.x);
00367     angle_base_target_camera = asin(neck_to_camera.z/target_xz_dist);
00368     
00369     tilt = angle_base_target - angle_base_target_camera + body_angle;
00370     tilt = bound(tilt,head_tilt_min,head_tilt_max);
00371     
00372     double camera_dist_to_target;
00373     camera_dist_to_target = sqrt(target_xz_dist*target_xz_dist - 
00374                                  neck_to_camera.z*neck_to_camera.z);
00375     
00376     pan = atan2(target.y,camera_dist_to_target);
00377     pan = bound(pan,head_pan_min,head_pan_max);
00378 
00379 #ifdef PLATFORM_APERIOS
00380     //pprintf(TextOutputStream,"txzd %g abt %g abtc %g tilt %g pan %g\n",
00381     //        target_xz_dist,angle_base_target,angle_base_target_camera,
00382     //        tilt,pan);
00383 #endif
00384   }
00385   else {
00386     tilt = pan = 0.0;
00387   }
00388 
00389   angles[0] = tilt;
00390   angles[1] = pan;
00391   angles[2] = 0.0; // roll
00392 }
00393 
00394 vector3d
00395 RunForwardModel(double *angles, double body_angle, double body_height, vector3d point) {
00396   double tilt=0.0,pan=0.0,roll=0.0;
00397 
00398   tilt = angles[0];
00399   pan  = angles[1];
00400   roll = angles[2];
00401 
00402   point = point.rotate_x(roll);
00403   point += neck_to_camera;
00404   point = point.rotate_z(pan);
00405   point = point.rotate_y(-tilt+body_angle);
00406 
00407   vector3d neck;
00408 
00409   neck = body_to_neck;
00410   neck = neck.rotate_y(body_angle);
00411   neck.z += body_height;
00412 
00413   point.z += neck.z;
00414 
00415   return point;
00416 }
00417 
00418 // calculates the pose of the camera
00419 // location  = location of camera in robot coordinates relative to point on ground under neck
00420 // direction = unit vector pointing in direction of camera
00421 // up        = unit vector pointing in direction of higher on image
00422 // right     = unit vector pointing in direction of more right on image
00423 void GetHeadPosition(vector3d &location, vector3d &direction, vector3d &up, vector3d &right,
00424                      double *angles, double body_angle, double body_height)
00425 {
00426   location = RunForwardModel(angles, body_angle, body_height, vector3d(0.0,0.0,0.0));
00427 
00428   vector3d image_x,image_y,image_z;
00429 
00430   image_x = RunForwardModel(angles, body_angle, body_height, vector3d(0.0,-1.0, 0.0));
00431   image_y = RunForwardModel(angles, body_angle, body_height, vector3d(0.0, 0.0, 1.0));
00432   image_z = RunForwardModel(angles, body_angle, body_height, vector3d(1.0, 0.0, 0.0));
00433 
00434   direction = image_z - location;
00435   direction = direction.norm();
00436 
00437   up = image_y - location;
00438   up = up.norm();
00439 
00440   right = image_x - location;
00441   right = right.norm();
00442 }
00443 
00444 /*! @file
00445  * @brief Functions to provide kinematics calculations
00446  * @author CMU RoboSoccer 2001-2002 (Creator)
00447  * 
00448  * @verbinclude CMPack_license.txt
00449  *
00450  * $Author: ejt $
00451  * $Name: tekkotsu-1_4_1 $
00452  * $Revision: 1.3 $
00453  * $State: Exp $
00454  * $Date: 2003/01/17 23:15:51 $
00455  */
00456 

Tekkotsu v1.4
Generated Sat Jul 19 00:06:30 2003 by Doxygen 1.3.2