Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/Actorics/RobotVertices.cpp

Go to the documentation of this 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 }