Robot Simulator of the Robotics Group for Self-Organization of Control  0.8.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
arm.h
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2005-2011 LpzRobots development team *
3  * Georg Martius <georg dot martius at web dot de> *
4  * Frank Guettler <guettler at informatik dot uni-leipzig dot de *
5  * Frank Hesse <frank at nld dot ds dot mpg dot de> *
6  * Ralf Der <ralfder at mis dot mpg dot de> *
7  * *
8  * This program is free software; you can redistribute it and/or modify *
9  * it under the terms of the GNU General Public License as published by *
10  * the Free Software Foundation; either version 2 of the License, or *
11  * (at your option) any later version. *
12  * *
13  * This program is distributed in the hope that it will be useful, *
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
16  * GNU General Public License for more details. *
17  * *
18  * You should have received a copy of the GNU General Public License *
19  * along with this program; if not, write to the *
20  * Free Software Foundation, Inc., *
21  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
22  * *
23  ***************************************************************************/
24 
25 #ifndef __ARM_H
26 #define __ARM_H
27 
28 #include <selforg/inspectable.h>
29 #include <list>
30 #include <vector>
31 #include <cmath>
32 
33 #include <string.h>
34 #include <iostream>
35 
36 #include <selforg/matrix.h>
37 #include "oderobot.h"
38 // include primitives (box, spheres, cylinders ...)
39 #include "primitive.h"
40 #include "oneaxisservo.h"
41 #include "joint.h"
42 #include "sensor.h"
43 
44 
45 #include "osgprimitive.h"
46 
47 using namespace matrix;
48 
49 namespace lpzrobots{
50 
51  typedef struct
52  {
53  double body_mass;
54  double body_height;
55  double body_width;
56  double body_depth;
57 
58  double shoulder_mass;
60  double joint_offset; // distance of shoulder components from each other
61 
62  double upperarm_mass;
65 
66  double forearm_mass;
69 
70  double elevation_min;
71  double elevation_max;
72  double humeral_min;
73  double humeral_max;
74  double azimuthal_min;
75  double azimuthal_max;
76  double elbow_min;
77  double elbow_max;
78 
79  double motorPower;
80  double damping; // motor damping
81  double servoFactor; // reduces servo angle constraints to servoFactor percent of hingeJoint angle constraints
83 
84  bool withContext; // if true context sensors are the effector positions
85  bool useJointSensors; // if true joint sensors otherwise effector positions
86 
87  std::list<Sensor*> sensors; // list of additional sensors
88 
89  } ArmConf;
90 
91  class Arm : public OdeRobot
92  {
93  public:
94 
95  /* Enumeration of different parts and joints */
96  enum parts
97  {
103  hand
104  };
105 
106  Arm(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const ArmConf& conf, const std::string& name);
107 
109  {
110  ArmConf conf;
111 
112  conf.motorPower=5;//2-15;
113  conf.damping=0.2;//1.0;
114 
115  conf.upperarm_radius = 0.05;//0.15; <- not beautiful
116  conf.forearm_radius = 0.05;//0.1; <- not beautiful TODO universelle Anordnung!
117 
118  // body
119  conf.body_mass = 1.0;
120  conf.body_height = 5.0;
121  conf.body_width = 2.0;
122  conf.body_depth = 0.5;
123  // shoulder
124  conf.shoulder_mass=0.005;
125  conf.shoulder_radius=0.03; // 0.1
126  conf.joint_offset=0.005;
127  // upper arm
128  conf.upperarm_mass = 0.1; // 0.01
129  conf.upperarm_length = 1.5;
130  // forearm
131  conf.forearm_mass = 0.1; // 0.01
132  conf.forearm_length = 1.2;
133  // stops at hinge joints
134  conf.elevation_min=-M_PI/3;
135  conf.elevation_max=M_PI/3;
136  conf.humeral_min=-M_PI/4;
137  conf.humeral_max=M_PI/4;
138  conf.azimuthal_min=-M_PI/4;
139  conf.azimuthal_max=M_PI/3;
140  conf.elbow_min=-M_PI/3.5; // 50Deg. the hard limit is at about 60
141  conf.elbow_max=M_PI/3.5; // 50Deg. the hard limit is at about 60
142  conf.servoFactor=1;
143  conf.scaleMotorElbow=0.6;
144  conf.useJointSensors=true;
145  conf.withContext=false;
146 
147  return conf;
148  }
149 
150  virtual ~Arm(){};
151 
152  virtual paramkey getName() const {return "Arm";};
153 
154  /**
155  * sets the pose of the vehicle
156  * @param pose desired 4x4 pose matrix
157  */
158  virtual void placeIntern(const osg::Matrix& pose);
159 
160  /**
161  * update the subcomponents
162  */
163  virtual void update();
164 
165  /**
166  * returns actual sensorvalues
167  * @param sensors sensors scaled to [-1,1]
168  * @param sensornumber length of the sensor array
169  * @return number of actually written sensors
170  */
171  virtual int getSensorsIntern(double* sensors, int sensornumber);
172 
173  /**
174  * sets actual motorcommands
175  * @param motors motors scaled to [-1,1]
176  * @param motornumber length of the motor array
177  */
178  virtual void setMotorsIntern(const double* motors, int motornumber);
179 
180  /**
181  * returns number of sensors
182  */
183  virtual int getSensorNumberIntern()
184  {
185  return sensorno;
186  };
187 
188  /**
189  * returns number of motors
190  */
191  virtual int getMotorNumberIntern()
192  {
193  return motorno;
194  };
195 
196  /**
197  * returns a vector with the positions of all segments of the robot
198  * @param poslist vector of positions (of all robot segments)
199  * @return length of the list
200  */
201  virtual int getSegmentsPosition(std::vector<Position> &poslist);
202 
203  /**
204  * returns the position of the endeffector (hand)
205  * @param position vector position vector
206  */
207  void getEndeffectorPosition(double* position);
208 
209  /**
210  * this function is called in each timestep. It should perform robot-internal checks,
211  * like space-internal collision detection, sensor resets/update etc.
212  * @param globalData structure that contains global data from the simulation environment
213  */
214  virtual void doInternalStuff(GlobalData& globalData);
215 
216  /******** CONFIGURABLE ***********/
217  virtual void notifyOnChange(const paramkey& key);
218 
219  virtual Primitive* getMainObject() const{
220  return objects[base];
221  }
222 
223  /**
224  * the main object of the robot, which is used for position and speed tracking
225  */
226  virtual Primitive* getMainPrimitive() const
227  {
228  return objects[hand];
229  }
230 
231  void setDlearnTargetHack(double* post);
232  void setDmotorTargetHack(double* post);
233 
234  protected:
235 
236  /**
237  * creates vehicle at desired pose
238  * @param pose 4x4 pose matrix
239  * @param snowmanmode snowman body
240  */
241  virtual void create(const osg::Matrix& pose);
242 
243  /**
244  * destroys vehicle and space
245  */
246  virtual void destroy();
247 
248  static void mycallback(void *data, dGeomID o1, dGeomID o2);
249 
250  void hitTarget();
251 
252  double dBodyGetPositionAll ( dBodyID basis , int para );
253  double dGeomGetPositionAll ( dGeomID basis , int para );
254 
255  void BodyCreate(int n, dMass m, dReal x, dReal y, dReal z, dReal qx, dReal qy, dReal qz, dReal qangle);
256 
257  // inspectable interface
258  //virtual std::list<Inspectable::iparamkey> getInternalParamNames() const;
259  //virtual std::list<Inspectable::iparamval> getInternalParams() const;
260  // virtual std::list<ILayer> getStructuralLayers() const;
261  // virtual std::list<IConnection> getStructuralConnections() const;
262 
263 
266 
269 
270 
271 
272  std::vector <HingeServo*> hingeServos;
273 
274  int sensorno; // number of sensors
275  int motorno; // number of motors
276 
277  bool created; // true if robot was created
278 
279  dSpaceID parentspace;
280 
281  int printed;
282 
283  };
284 }
285 #endif
Matrix type.
Definition: matrix.h:65
double damping
Definition: arm.h:80
double humeral_min
Definition: arm.h:72
std::vector< HingeServo * > hingeServos
Definition: arm.h:272
double upperarm_mass
Definition: arm.h:62
double shoulder_radius
Definition: arm.h:59
Data structure for accessing the ODE.
Definition: odehandle.h:44
bool useJointSensors
Definition: arm.h:85
std::string paramkey
Definition: configurable.h:85
double elevation_max
Definition: arm.h:71
double body_mass
Definition: arm.h:53
matrix::Matrix endeff
Definition: arm.h:265
Matrixd Matrix
Definition: osgforwarddecl.h:47
parts
Definition: arm.h:96
charArray paramkey
Definition: avrtypes.h:36
double joint_offset
Definition: arm.h:60
double body_height
Definition: arm.h:54
double forearm_radius
Definition: arm.h:67
virtual paramkey getName() const
return the name of the object
Definition: arm.h:152
double upperarm_length
Definition: arm.h:64
Definition: arm.h:51
Data structure for accessing the OpenSceneGraph.
Definition: osghandle.h:79
double upperarm_radius
Definition: arm.h:63
Definition: arm.h:98
virtual int getSensorNumberIntern()
returns number of sensors
Definition: arm.h:183
Definition: arm.h:101
virtual Primitive * getMainPrimitive() const
the main object of the robot, which is used for position and speed tracking
Definition: arm.h:226
Definition: arm.h:100
static ArmConf getDefaultConf()
Definition: arm.h:108
double paramval
Definition: configurable.h:88
virtual Primitive * getMainObject() const
Definition: arm.h:219
double azimuthal_max
Definition: arm.h:75
Interface class for primitives represented in the physical and graphical world.
Definition: primitive.h:80
double humeral_max
Definition: arm.h:73
double forearm_mass
Definition: arm.h:66
double forearm_length
Definition: arm.h:68
Data structure holding all essential global information.
Definition: globaldata.h:57
double motorPower
Definition: arm.h:79
double elbow_max
Definition: arm.h:77
dSpaceID parentspace
Definition: arm.h:279
double body_depth
Definition: arm.h:56
bool created
Definition: arm.h:277
virtual ~Arm()
Definition: arm.h:150
double shoulder_mass
Definition: arm.h:58
Definition: arm.h:91
bool withContext
Definition: arm.h:84
paramval print
Definition: arm.h:268
double elevation_min
Definition: arm.h:70
paramval factorSensors
Definition: arm.h:267
int sensorno
Definition: arm.h:274
int printed
Definition: arm.h:281
Abstract class for ODE robots.
Definition: oderobot.h:64
Definition: arm.h:102
std::list< Sensor * > sensors
Definition: arm.h:87
virtual int getMotorNumberIntern()
returns number of motors
Definition: arm.h:191
double servoFactor
Definition: arm.h:81
Definition: arm.h:99
double elbow_min
Definition: arm.h:76
ArmConf conf
Definition: arm.h:264
double scaleMotorElbow
Definition: arm.h:82
double body_width
Definition: arm.h:55
int motorno
Definition: arm.h:275
double azimuthal_min
Definition: arm.h:74