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.h File Reference


Detailed Description

Functions to provide kinematics calculations.

Author:
CMU RoboSoccer 2001-2002 (Creator)
  =========================================================================
    CMPack'02 Source Code Release for OPEN-R SDK v1.0
    Copyright (C) 2002 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
  -------------------------------------------------------------------------
    This software is distributed under the GNU General Public License,
    version 2.  If you do not have a copy of this licence, visit
    www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
    Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
    in the hope that it will be useful, but WITHOUT ANY WARRANTY,
    including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  -------------------------------------------------------------------------
    Additionally licensed to Sony Corporation under the following terms:

    This software is provided by the copyright holders AS IS and any
    express or implied warranties, including, but not limited to, the
    implied warranties of merchantability and fitness for a particular
    purpose are disclaimed.  In no event shall authors be liable for
    any direct, indirect, incidental, special, exemplary, or consequential
    damages (including, but not limited to, procurement of substitute
    goods or services; loss of use, data, or profits; or business
    interruption) however caused and on any theory of liability, whether
    in contract, strict liability, or tort (including negligence or
    otherwise) arising in any way out of the use of this software, even if
    advised of the possibility of such damage.
  =========================================================================

Author
ejt
Name
tekkotsu-1_4_1
Revision
1.6
State
Exp
Date
2003/03/03 01:18:13

Definition in file Kinematics.h.

#include "Geometry.h"
#include "Shared/Util.h"

Include dependency graph for Kinematics.h:

Include dependency graph

This graph shows which files directly or indirectly include this file:

Included by dependency graph

Go to the source code of this file.

Compounds

struct  BodyPosition
 holds the current location of the body, as a delta from when walking started More...


Defines

#define RAD(deg)   (((deg) * M_PI ) / 180.0)
#define DEG(rad)   (((rad) * 180.0) / M_PI )

Functions

const vector3d body_to_neck (75.00, 0.00, 50.00)
const vector3d neck_to_camera (65.00, 0.00, 48.00)
void KinClearErrors ()
int KinGetErrors ()
void GetLegAngles (double *angles, vector3d target, int leg)
void GetLegAngles (double *angles, vector3d target[4], double body_angle, double body_height)
void GetLegAngles (double *angles, vector3d target[4], BodyPosition &bp)
void GetLegAngles (double *angles, vector3d target, BodyPosition &bp, int leg)
void GetLegPosition (vector3d &p, const double *ang, int leg)
void GetBodyLocation (vector3d &ball, vector3d &toe, const double *ang, int leg)
void GetHeadAngles (double *angles, vector3d target, double body_angle, double body_height)
vector3d RunForwardModel (double *angles, double body_angle, double body_height, vector3d point)
void GetHeadPosition (vector3d &location, vector3d &direction, vector3d &up, vector3d &right, double *angles, double body_angle, double body_height)

Variables

const double rotator_min
const double rotator_max
const double shoulder_min
const double shoulder_max
const double knee_max
const double knee_min
const double rotator_kmin
const double rotator_kmax
const double shoulder_kmin
const double shoulder_kmax
const double knee_kmax
const double f_knee_kmin
const double h_knee_kmin
const double tail_min
const double tail_max
const double head_tilt_min
const double head_tilt_max
const double head_pan_min
const double head_pan_max
const double head_roll_min
const double head_roll_max


Define Documentation

#define DEG rad   )     (((rad) * 180.0) / M_PI )
 

Definition at line 38 of file Kinematics.h.

#define RAD deg   )     (((deg) * M_PI ) / 180.0)
 

Definition at line 37 of file Kinematics.h.


Function Documentation

const vector3d body_to_neck 75.  00,
0.  00,
50.  00
 

void GetBodyLocation vector3d ball,
vector3d toe,
const double *  ang,
int  leg
 

Definition at line 315 of file Kinematics.cc.

References f_body_to_shoulder(), f_leg_knee_to_ball(), f_leg_shoulder_to_knee(), h_body_to_shoulder(), h_leg_knee_to_ball(), h_leg_shoulder_to_knee(), GVector::vector3d< double >::rotate_x(), GVector::vector3d< double >::rotate_y(), and GVector::vector3d< double >::y.

void GetHeadAngles double *  angles,
vector3d  target,
double  body_angle,
double  body_height
 

Definition at line 337 of file Kinematics.cc.

References body_to_neck(), bound(), head_pan_max, head_pan_min, head_tilt_max, head_tilt_min, GVector::vector3d< double >::length(), neck_to_camera(), GVector::vector3d< double >::rotate_y(), GVector::vector3d< double >::x, GVector::vector3d< double >::y, and GVector::vector3d< double >::z.

void GetHeadPosition vector3d location,
vector3d direction,
vector3d up,
vector3d right,
double *  angles,
double  body_angle,
double  body_height
 

Definition at line 423 of file Kinematics.cc.

References GVector::vector3d< double >::norm(), RunForwardModel(), and vector3d.

void GetLegAngles double *  angles,
vector3d  target,
BodyPosition bp,
int  leg
 

Definition at line 289 of file Kinematics.cc.

References BodyPosition::angle, BodyPosition::loc, GVector::vector3d< double >::rotate_z(), GVector::vector3d< double >::y, and GVector::vector3d< double >::z.

void GetLegAngles double *  angles,
vector3d  target[4],
BodyPosition bp
 

Definition at line 278 of file Kinematics.cc.

References BodyPosition::angle, GetLegAngles(), BodyPosition::loc, GVector::vector3d< double >::rotate_z(), GVector::vector3d< double >::y, and GVector::vector3d< double >::z.

void GetLegAngles double *  angles,
vector3d  target[4],
double  body_angle,
double  body_height
 

Definition at line 264 of file Kinematics.cc.

References GetLegAngles(), GVector::vector3d< double >::rotate_y(), and GVector::vector3d< double >::z.

void GetLegAngles double *  angles,
vector3d  target,
int  leg
 

Definition at line 185 of file Kinematics.cc.

References f_body_to_shoulder(), f_knee_kmin, f_leg_knee_to_ball(), f_leg_shoulder_to_knee(), f_lower, f_upper, GetTrigAngle(), h_body_to_shoulder(), h_knee_kmin, h_leg_knee_to_ball(), h_leg_shoulder_to_knee(), h_lower, h_upper, knee_kmax, GVector::vector3d< double >::rotate_y(), rotator_max, rotator_min, shoulder_kmax, shoulder_kmin, GVector::vector3d< double >::sqlength(), GVector::vector3d< double >::x, GVector::vector3d< double >::y, and GVector::vector3d< double >::z.

void GetLegPosition vector3d p,
const double *  ang,
int  leg
 

Definition at line 298 of file Kinematics.cc.

References f_body_to_shoulder(), f_leg_knee_to_ball(), f_leg_shoulder_to_knee(), h_body_to_shoulder(), h_leg_knee_to_ball(), h_leg_shoulder_to_knee(), GVector::vector3d< double >::rotate_x(), GVector::vector3d< double >::rotate_y(), and GVector::vector3d< double >::y.

void KinClearErrors  ) 
 

Definition at line 91 of file Kinematics.cc.

References errors.

int KinGetErrors  ) 
 

Definition at line 96 of file Kinematics.cc.

References errors.

const vector3d neck_to_camera 65.  00,
0.  00,
48.  00
 

vector3d RunForwardModel double *  angles,
double  body_angle,
double  body_height,
vector3d  point
 

Definition at line 395 of file Kinematics.cc.

References body_to_neck(), neck_to_camera(), GVector::vector3d< double >::rotate_x(), GVector::vector3d< double >::rotate_y(), GVector::vector3d< double >::rotate_z(), and GVector::vector3d< double >::z.


Variable Documentation

const double f_knee_kmin
 

Definition at line 52 of file Kinematics.h.

const double h_knee_kmin
 

Definition at line 53 of file Kinematics.h.

const double head_pan_max
 

Definition at line 61 of file Kinematics.h.

const double head_pan_min
 

Definition at line 60 of file Kinematics.h.

const double head_roll_max
 

Definition at line 63 of file Kinematics.h.

const double head_roll_min
 

Definition at line 62 of file Kinematics.h.

const double head_tilt_max
 

Definition at line 59 of file Kinematics.h.

const double head_tilt_min
 

Definition at line 58 of file Kinematics.h.

const double knee_kmax
 

Definition at line 51 of file Kinematics.h.

const double knee_max
 

Definition at line 44 of file Kinematics.h.

const double knee_min
 

Definition at line 45 of file Kinematics.h.

const double rotator_kmax
 

Definition at line 48 of file Kinematics.h.

const double rotator_kmin
 

Definition at line 47 of file Kinematics.h.

const double rotator_max
 

Definition at line 41 of file Kinematics.h.

const double rotator_min
 

Definition at line 40 of file Kinematics.h.

const double shoulder_kmax
 

Definition at line 50 of file Kinematics.h.

const double shoulder_kmin
 

Definition at line 49 of file Kinematics.h.

const double shoulder_max
 

Definition at line 43 of file Kinematics.h.

const double shoulder_min
 

Definition at line 42 of file Kinematics.h.

const double tail_max
 

Definition at line 56 of file Kinematics.h.

const double tail_min
 

Definition at line 55 of file Kinematics.h.


Tekkotsu v1.4
Generated Sat Jul 19 00:07:27 2003 by Doxygen 1.3.2