iCub-main
OdeLogicalJoint.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
5 * Author: Vadim Tikhanoff, Paul Fitzpatrick
6 * email: vadim.tikhanoff@iit.it, paulfitz@alum.mit.edu
7 * website: www.robotcub.org
8 * Permission is granted to copy, distribute, and/or modify this program
9 * under the terms of the GNU General Public License, version 2 or any
10 * later version published by the Free Software Foundation.
11 *
12 * A copy of the license can be found at
13 * http://www.robotcub.org/icub/license/gpl.txt
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18 * Public License for more details
19 */
20 
21 #include "OdeLogicalJoint.h"
22 #include "OdeInit.h"
23 
24 #include <stdio.h>
25 #include <stdlib.h>
26 #include <yarp/os/LogStream.h>
27 
28 using namespace std;
29 
30 OdeLogicalJoint::OdeLogicalJoint() : filter(6,0.3,0.0,100) {
31  sub = NULL;
32  joint = NULL;
33  speed = NULL;
34  feedback = NULL;
35  active = false;
36  left = NULL;
37  right = NULL;
38  verge = 0;
39  speedSetpoint = 0;
40  number = -1;
41  vel = 1;
42  acc = 1;
43 }
44 
45 
46 
48  if (sub!=NULL) {
49  delete[] sub;
50  sub = NULL;
51  }
52 }
53 
55  if (sub!=NULL) {
56  delete[] sub;
57  sub = NULL;
58  }
59  sub = new OdeLogicalJoint[len];
60  subLength = len;
61  return sub;
62 }
63 
65  if (sub!=NULL) {
66  return sub+index;
67  }
68  return NULL;
69 }
70 
72  OdeLogicalJoint& right,
73  OdeLogicalJoint& peer,
74  int sgn) {
75  yDebug("Vergence motor remap %d\n", sgn);
76  active = true;
77  verge = sgn;
78  sign = 1;
79  this->left = &left;
80  this->right = &right;
81  this->peer = &peer;
82 }
83 
84 
85 void OdeLogicalJoint::init(const char *unit,
86  const char *type,
87  int index,
88  int sign, RobotConfig &conf) {
89  OdeInit& odeinit = OdeInit::get();
90  active = true;
91  number = index;
92  this->sign = sign;
93  this->unit = unit;
94  universal = 0;
95  string sunit = unit;
96  string stype = type;
97  yDebug("Motor %s %s %d %d\n", unit, type, index, sign);
98  if (stype=="hinge") {
99  hinged = true;
100  universal = 0;
101  } else if (stype=="universalAngle1") {
102  hinged = false;
103  universal = 1;
104  } else if (stype=="universalAngle2") {
105  hinged = false;
106  universal = 2;
107  } else {
108  yError("Unknown axis type %s\n", type);
109  ::exit(1);
110  }
111  if (sunit=="leftarm") {
112  joint = &(odeinit._iCub->LAjoints[index]);
113  if (hinged) {
114  speed = &(odeinit._iCub->la_speed[index]);
115  } else if (universal==2) {
116  speed = &(odeinit._iCub->la_speed1[index]);
117  } else {
118  speed = &(odeinit._iCub->la_speed[index]);
119  }
120  torque = &(odeinit._iCub->la_torques[index]);
121  } else if (sunit=="rightarm") {
122  joint = &(odeinit._iCub->RAjoints[index]);
123  if (hinged) {
124  speed = &(odeinit._iCub->ra_speed[index]);
125  } else if (universal==2) {
126  speed = &(odeinit._iCub->ra_speed1[index]);
127  } else {
128  speed = &(odeinit._iCub->ra_speed[index]);
129  }
130  torque = &(odeinit._iCub->ra_torques[index]);
131  }
132  else if (sunit=="head") {
133  joint = &(odeinit._iCub->Hjoints[index]);
134  speed = &(odeinit._iCub->h_speed[index]);
135  torque = &(odeinit._iCub->h_torques[index]);
136  }
137  else if (sunit=="leftleg") {
138  joint = &(odeinit._iCub->LLegjoints[index]);
139  if (hinged) {
140  speed = &(odeinit._iCub->LLeg_speed[index]);
141  }
142  torque = &(odeinit._iCub->LLeg_torques[index]);
143  }
144  else if (sunit=="rightleg") {
145  joint = &(odeinit._iCub->RLegjoints[index]);
146  if (hinged) {
147  speed = &(odeinit._iCub->RLeg_speed[index]);
148  }
149  torque = &(odeinit._iCub->RLeg_torques[index]);
150  }
151  else if (sunit=="torso") {
152  joint = &(odeinit._iCub->Torsojoints[index]);
153  if (hinged) {
154  speed = &(odeinit._iCub->Torso_speed[index]);
155  }
156  torque = &(odeinit._iCub->Torso_torques[index]);
157  }
158  else {
159  yError("Unknown body unit %s\n", unit);
160  ::exit(1);
161  }
162  feedback = new dJointFeedback;
163  dJointSetFeedback(*joint, feedback);
164  if(hinged)
165  dJointGetHingeAxis(*joint, axis);
166  else if(universal==1)
167  dJointGetUniversalAxis1(*joint, axis);
168  else if(universal==2)
169  dJointGetUniversalAxis2(*joint, axis);
170 
171  dryFriction = conf.getMotorDryFriction();
172  maxTorque = conf.getMotorMaxTorque();
173 }
174 
176  // odeinit._iCub->torqueData[0]
177  if(!hinged || !feedback)
178  return 0.0;
179  return *torque; //(feedback->t1[0]*axis[0]+feedback->t1[1]*axis[1]+feedback->t1[2]*axis[2]);
180 }
181 
182 void OdeLogicalJoint::setTorque(double target){
183  *torque = target;
184 }
185 
187  if (verge==0) {
188  if (hinged) {
189  return dJointGetHingeAngle(*joint);
190  } else if (universal == 1) {
191  return dJointGetUniversalAngle1(*joint);
192  } else if (universal == 2) {
193  return dJointGetUniversalAngle2(*joint);
194  }
195  return 0;
196  }
197 
198  return (left->getAngleRaw() + verge*right->getAngleRaw()) * (verge==-1?1.0:0.5);
199 }
200 
201 
203  if (verge==0) {
204  if (joint == NULL){
205  return 0;
206  }
207  if (hinged) {
208  return dJointGetHingeAngleRate(*joint);
209  } else if (universal == 1) {
210  return dJointGetUniversalAngle1Rate(*joint);
211  } else if (universal == 2) {
212  return dJointGetUniversalAngle2Rate(*joint);
213  }
214  return 0;
215  }
216 
217  return (left->getVelocityRaw() + verge*right->getVelocityRaw()) * (verge==-1?1.0:0.5);
218 }
219 
220 
221 void OdeLogicalJoint::setControlParameters(double vel, double acc) {
222  this->vel = vel;
223  this->acc = acc;
224  if (sub!=NULL) {
225  for (int i=0; i<subLength; i++) {
226  sub[i].setControlParameters(vel,acc);
227  }
228  }
229  if (verge==1) {
230  left->setControlParameters(vel,acc);
231  right->setControlParameters(vel,acc);
232  }
233 }
234 
235 
236 void OdeLogicalJoint::setPosition(double target) {
237  double error = target - getAngleRaw()*sign;
238  double ctrl = filter.pid(error);
239  setVelocityRaw(ctrl*sign*vel);
240  if (sub!=NULL) {
241  for (int i=0; i<subLength; i++) {
242  sub[i].setPosition(target);
243  }
244  }
245 }
246 
247 void OdeLogicalJoint::setVelocity(double target) {
248  if (sub!=NULL) {
249  for (int i=0; i<subLength; i++) {
250  sub[i].setVelocity(target);
251  }
252  }
253  setVelocityRaw(sign*target);
254 }
255 
256 
257 void OdeLogicalJoint::setVelocityRaw(double target) {
258  speedSetpoint = target;
259  if (verge==0) {
260  if (speed != NULL){
261  (*speed) = speedSetpoint;
262  }
263  } else {
264  if (fabs(target)>0.1) {
265  //yDebug("verger %d velocity %g\n", verge, target);
266  }
267  double altSpeed = peer->getSpeedSetpoint();
268  if (verge==1) {
269  left->setVelocityRaw(speedSetpoint+altSpeed);
270  right->setVelocityRaw(speedSetpoint-altSpeed);
271  }
272  }
273 }
274 
275 
277  if(hinged){
278  // only hinge joints can be torque controlled
279  if(cm==MODE_TORQUE){
280  dJointSetHingeParam(*joint, dParamFMax, dryFriction);
281  dJointSetHingeParam(*joint, dParamVel, 0.0);
282  }
283  else{
284  dJointSetHingeParam(*joint, dParamFMax, maxTorque);
285  }
286  }
287 }
288 
289 
This file is responsible for the initialisation of the world parameters that are controlled by ODE.
dReal ra_speed1[25]
Definition: iCub.h:217
dReal h_torques[25]
Definition: iCub.h:226
dReal LLeg_speed[20]
Definition: iCub.h:211
dJointID LLegjoints[20]
Definition: iCub.h:194
dReal RLeg_speed[20]
Definition: iCub.h:212
dReal la_speed1[25]
Definition: iCub.h:215
dReal ra_torques[25]
Definition: iCub.h:225
dReal ra_speed[25]
Definition: iCub.h:216
dJointID RLegjoints[20]
Definition: iCub.h:195
dReal h_speed[25]
Definition: iCub.h:218
dReal la_speed[25]
Definition: iCub.h:214
dReal Torso_speed[8]
Definition: iCub.h:213
dReal LLeg_torques[20]
Definition: iCub.h:221
dJointID RAjoints[25]
Definition: iCub.h:199
dJointID Torsojoints[8]
Definition: iCub.h:197
dReal Torso_torques[8]
Definition: iCub.h:223
dReal RLeg_torques[20]
Definition: iCub.h:222
dJointID Hjoints[6]
Definition: iCub.h:201
dReal la_torques[25]
Definition: iCub.h:224
dJointID LAjoints[25]
Definition: iCub.h:198
ODE state information.
Definition: OdeInit.h:55
ICubSim * _iCub
Definition: OdeInit.h:67
static OdeInit & get()
Definition: OdeInit.cpp:189
Convenience class for mapping from physical ODE joints in the model to control joints in the ICUB spe...
OdeLogicalJoint * at(int index)
Access a nested control unit.
void controlModeChanged(int cm)
void init(const char *unit, const char *type, int index, int sign, RobotConfig &conf)
Initialize a regular control unit.
virtual ~OdeLogicalJoint()
Destructor.
void setPosition(double target)
Drive towards an angle setpoint (in ICUB units/sign).
OdeLogicalJoint * nest(int len)
Create an array of nested control units.
double getAngleRaw()
Get the angle of an associated joint in unconverted units and sign.
double getVelocityRaw()
Get the velocity of an associated joint in unconverted units and sign.
double getTorque()
this is just a fake torque interface for now
void setVelocityRaw(double target)
Set raw velocity of joint (in ODE units/sign).
OdeLogicalJoint()
Constructor.
void setVelocity(double target)
Set velocity of joint (in ICUB units/sign).
void setControlParameters(double vel, double acc)
Set velocity and acceleration control parameters.
void setTorque(double target)
Set the reference torque.
double getSpeedSetpoint()
Get the current target velocity.
double pid(double error)
Definition: pidfilter.h:74
virtual double getMotorMaxTorque()=0
virtual double getMotorDryFriction()=0
_3f_vect_t acc
Definition: dataTypes.h:1
#define MODE_TORQUE