GT2004: Tools/Actorics/RobotVertices.cpp Source File

00001 /**
00002 * @file RobotVertices.cpp
00003 *
00004 * Implementation of struct RobotVertices
00005 * 
00006 * @author Uwe Düffert
00007 */
00008 
00009 #include "RobotVertices.h"
00010 #include "Tools/Math/Common.h"
00011 
00012 RobotVertices::RobotVertices(const RobotDimensions& r):bodyTilt(0),bodyRoll(0),neckHeight(0),frameNumber(0)
00013 {
00014   neck=Vector3<double>(0,0,r.lowerForeLegLength+r.upperLegLength+r.neckToLegsLengthZ);
00015 
00016   //order is the same as in Kinematics::LegIndex{fr,fl,hr,hl}:
00017   footPosition[0]=Vector3<double>(-r.neckToLegsLengthX,-r.bodyWidth/2,0);
00018   footPosition[1]=Vector3<double>(-r.neckToLegsLengthX,r.bodyWidth/2,0);
00019   footPosition[2]=Vector3<double>(-r.neckToLegsLengthX-r.lengthBetweenLegs,-r.bodyWidth/2,0);
00020   footPosition[3]=Vector3<double>(-r.neckToLegsLengthX-r.lengthBetweenLegs,r.bodyWidth/2,0);
00021 
00022   kneePosition[0]=footPosition[0]+Vector3<double>(r.upperLegLengthX,0,cos(r.zeroFrontKneeArc-r.zeroShoulderArc)*r.lowerForeLegLength);
00023   kneePosition[1]=footPosition[1]+Vector3<double>(r.upperLegLengthX,0,cos(r.zeroFrontKneeArc-r.zeroShoulderArc)*r.lowerForeLegLength);
00024   kneePosition[2]=footPosition[2]+Vector3<double>(-r.upperLegLengthX,0,cos(r.zeroHindKneeArc-r.zeroShoulderArc)*r.lowerHindLegLength);
00025   kneePosition[3]=footPosition[3]+Vector3<double>(-r.upperLegLengthX,0,cos(r.zeroHindKneeArc-r.zeroShoulderArc)*r.lowerHindLegLength);
00026 
00027   shoulderPosition[0]=kneePosition[0]+Vector3<double>(-r.upperLegLengthX,0,cos(r.zeroShoulderArc)*r.upperLegLength);
00028   shoulderPosition[1]=kneePosition[1]+Vector3<double>(-r.upperLegLengthX,0,cos(r.zeroShoulderArc)*r.upperLegLength);
00029   shoulderPosition[2]=kneePosition[2]+Vector3<double>(r.upperLegLengthX,0,cos(r.zeroShoulderArc)*r.upperLegLength);
00030   shoulderPosition[3]=kneePosition[3]+Vector3<double>(r.upperLegLengthX,0,cos(r.zeroShoulderArc)*r.upperLegLength);
00031 }
00032 
00033 In& operator>>(In& stream,RobotVertices& robotVertices)
00034 {
00035   stream.read(&robotVertices,sizeof(robotVertices));
00036   return stream;
00037 }
00038  
00039 Out& operator<<(Out& stream, const RobotVertices& robotVertices)
00040 {
00041   stream.write(&robotVertices,sizeof(robotVertices));
00042   return stream;
00043 }
00044 
00045 /*
00046  * Change log :
00047  * 
00048  * $Log: RobotVertices.cpp,v $
00049  * Revision 1.1.1.1  2004/05/22 17:36:01  cvsadm
00050  * created new repository GT2004_WM
00051  *
00052  * Revision 1.5  2004/03/20 09:55:28  roefer
00053  * Preparation for improved odometry
00054  *
00055  * Revision 1.4  2004/01/28 21:55:50  roefer
00056  * RobotDimensions revised
00057  *
00058  * Revision 1.3  2004/01/01 10:58:51  roefer
00059  * RobotDimensions are in a class now
00060  *
00061  * Revision 1.2  2003/12/02 18:14:34  dueffert
00062  * correction values named
00063  *
00064  * Revision 1.1  2003/10/07 10:13:21  cvsadm
00065  * Created GT2004 (M.J.)
00066  *
00067  * Revision 1.1  2003/09/26 11:40:40  juengel
00068  * - sorted tools
00069  * - clean-up in DataTypes
00070  *
00071  * Revision 1.1  2003/09/15 20:37:07  dueffert
00072  * new DataType added
00073  *
00074  */