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 
dReal ra_speed1[25]
Definition: iCub.h:217
double pid(double error)
Definition: pidfilter.h:74
#define MODE_TORQUE
dReal RLeg_torques[20]
Definition: iCub.h:222
Convenience class for mapping from physical ODE joints in the model to control joints in the ICUB spe...
OdeLogicalJoint * nest(int len)
Create an array of nested control units.
dReal Torso_torques[8]
Definition: iCub.h:223
dReal LLeg_torques[20]
Definition: iCub.h:221
ICubSim * _iCub
Definition: OdeInit.h:67
void setVelocity(double target)
Set velocity of joint (in ICUB units/sign).
STL namespace.
dReal h_torques[25]
Definition: iCub.h:226
double getSpeedSetpoint()
Get the current target velocity.
OdeLogicalJoint()
Constructor.
virtual double getMotorMaxTorque()=0
dJointID LAjoints[25]
Definition: iCub.h:198
static OdeInit & get()
Definition: OdeInit.cpp:189
dReal ra_speed[25]
Definition: iCub.h:216
void setVelocityRaw(double target)
Set raw velocity of joint (in ODE units/sign).
void setPosition(double target)
Drive towards an angle setpoint (in ICUB units/sign).
void setTorque(double target)
Set the reference torque.
This file is responsible for the initialisation of the world parameters that are controlled by ODE...
ODE state information.
Definition: OdeInit.h:55
void init(const char *unit, const char *type, int index, int sign, RobotConfig &conf)
Initialize a regular control unit.
dJointID LLegjoints[20]
Definition: iCub.h:194
dJointID RLegjoints[20]
Definition: iCub.h:195
double getAngleRaw()
Get the angle of an associated joint in unconverted units and sign.
virtual ~OdeLogicalJoint()
Destructor.
dReal LLeg_speed[20]
Definition: iCub.h:211
OdeLogicalJoint * at(int index)
Access a nested control unit.
dReal la_speed1[25]
Definition: iCub.h:215
dJointID Hjoints[6]
Definition: iCub.h:201
virtual double getMotorDryFriction()=0
double getTorque()
this is just a fake torque interface for now
double getVelocityRaw()
Get the velocity of an associated joint in unconverted units and sign.
dReal la_speed[25]
Definition: iCub.h:214
dJointID RAjoints[25]
Definition: iCub.h:199
void setControlParameters(double vel, double acc)
Set velocity and acceleration control parameters.
void controlModeChanged(int cm)
dReal RLeg_speed[20]
Definition: iCub.h:212
dReal Torso_speed[8]
Definition: iCub.h:213
dReal ra_torques[25]
Definition: iCub.h:225
dReal la_torques[25]
Definition: iCub.h:224
dReal h_speed[25]
Definition: iCub.h:218
dJointID Torsojoints[8]
Definition: iCub.h:197