iCub-main
iCubLogicalJoints.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 "iCubLogicalJoints.h"
22 #include <yarp/os/LogStream.h>
23 #include <stdlib.h>
24 
26  RobotFlags& flags = config.getFlags();
27  if (!flags.valid) {
28  yError("Robot flags are not set when creating iCubLogicalJoints\n");
29  ::exit(1);
30  }
31 
33  // Setting up the head
34 
35  int head = PART_HEAD;
36  int rawHead = PART_HEAD_RAW;
37  if (flags.actHead){
38  const char *headName = "head";
39  // for (int i=0; i<4; i++) {
40  getController(head,0).init(headName,"hinge",0,+1, config);
41  getController(head,1).init(headName,"hinge",1,-1, config);
42  getController(head,2).init(headName,"hinge",2,-1, config);
43  getController(head,3).init(headName,"hinge",3,+1, config);
44  // }
45  // for the eyes, we need to map vergence/version onto
46  // individual hinge joints
47  getController(rawHead,4).init(headName,"hinge",4,-1, config);
48  getController(rawHead,5).init(headName,"hinge",5,+1, config);
49 
50  getController(head,4).init(getController(rawHead,4),
51  getController(rawHead,5),
52  getController(head,5),
53  +1);
54  getController(head,5).init(getController(rawHead,4),
55  getController(rawHead,5),
56  getController(head,4),
57  -1);
58  }
60  // Setting up the left and right arms
61 
62  for (int arm = PART_ARM_LEFT; arm <= PART_ARM_RIGHT; arm++) {
63  const char *armName = (arm==PART_ARM_LEFT)?"leftarm":"rightarm";
64 
65  if (arm == PART_ARM_RIGHT && flags.actRArm){
66  getController(arm,0).init(armName,"hinge",0,-1, config);
67  getController(arm,1).init(armName,"hinge",1,+1, config);
68  getController(arm,2).init(armName,"hinge",2,-1, config);
69  getController(arm,3).init(armName,"hinge",3,+1, config);
70  getController(arm,4).init(armName,"hinge",4,-1, config);
71  getController(arm,5).init(armName,"universalAngle1",5,-1, config);
72  getController(arm,6).init(armName,"universalAngle2",5,-1, config);
73  }
74 
75  if (arm == PART_ARM_LEFT && flags.actLArm){
76  getController(arm,0).init(armName,"hinge",0,-1, config);
77  getController(arm,1).init(armName,"hinge",1,-1, config);
78  getController(arm,2).init(armName,"hinge",2,+1, config);
79  getController(arm,3).init(armName,"hinge",3,+1, config);
80  getController(arm,4).init(armName,"hinge",4,+1, config);
81  getController(arm,5).init(armName,"universalAngle1",5,+1, config);
82  getController(arm,6).init(armName,"universalAngle2",5,-1, config);
83 
84  }
85  if (arm == PART_ARM_RIGHT && flags.actRHand){
86  getController(arm,7).init(armName,"hinge",6,+1, config);
87  OdeLogicalJoint *sub = getController(arm,7).nest(1);
88  sub[0].init(armName,"hinge",8,-1, config);
89 
90  getController(arm,8).init(armName,"universalAngle1",22,-1, config);//thumb
91  getController(arm,9).init(armName,"universalAngle2",22,-1, config);//thumb
92 
93  getController(arm,10).init(armName,"hinge",23,-1, config);
94  sub = getController(arm,10).nest(1);
95  sub[0].init(armName,"hinge",24,-1, config);
96 
97  getController(arm,11).init(armName,"hinge",10,-1, config);//index proximal
98  getController(arm,12).init(armName,"hinge",14,-1, config);
99  sub = getController(arm,12).nest(1);
100  sub[0].init(armName,"hinge",18,-1, config);
101 
102  getController(arm,13).init(armName,"hinge",11,-1, config);//middle finger
103  getController(arm,14).init(armName,"hinge",15,-1, config);
104  sub = getController(arm,14).nest(1);
105  sub[0].init(armName,"hinge",19,-1, config);
106 
107  getController(arm,15).init(armName,"hinge",12,-1, config);//ring + pinky
108  sub = getController(arm,15).nest(2);
109  sub[0].init(armName,"hinge",16,-1, config);
110  sub[1].init(armName,"hinge",20,-1, config);
111  }
112  if (arm == PART_ARM_LEFT && flags.actLHand ){
113  getController(arm,7).init(armName,"hinge",6,+1, config);
114  OdeLogicalJoint *sub = getController(arm,7).nest(1);
115  sub[0].init(armName,"hinge",8,-1, config);
116 
117  /*getController(arm,8).init(armName,"hinge",22,+1);//thumb
118  getController(arm,9).init(armName,"hinge",23,+1);
119  getController(arm,10).init(armName,"hinge",24,+1);
120  */
121  getController(arm,8).init(armName,"universalAngle1",22,+1, config);//thumb
122  getController(arm,9).init(armName,"universalAngle2",22,-1, config);//thumb
123 
124  getController(arm,10).init(armName,"hinge",23,+1, config);
125  sub = getController(arm,10).nest(1);
126  sub[0].init(armName,"hinge",24,+1, config);
127 
128  getController(arm,11).init(armName,"hinge",10,+1, config);//index proximal
129  getController(arm,12).init(armName,"hinge",14,+1, config);
130  sub = getController(arm,12).nest(1);
131  sub[0].init(armName,"hinge",18,+1, config);
132 
133  getController(arm,13).init(armName,"hinge",11,+1, config);//middle finger
134  getController(arm,14).init(armName,"hinge",15,+1, config);
135  sub = getController(arm,14).nest(1);
136  sub[0].init(armName,"hinge",19,+1, config);
137 
138  getController(arm,15).init(armName,"hinge",12,+1, config);//ring + pinky
139  sub = getController(arm,15).nest(2);
140  sub[0].init(armName,"hinge",16,+1, config);
141  sub[1].init(armName,"hinge",20,+1, config);
142  }
143  }
144 
145  if (flags.actLegs){
146  for (int leg = PART_LEG_LEFT; leg <= PART_LEG_RIGHT; leg++) {
147  const char *legName = (leg==PART_LEG_LEFT)?"leftleg":"rightleg";
148  // changed for demo look below for previous joint setup
149  if (leg == PART_LEG_RIGHT){
150  getController(leg,0).init(legName,"hinge",5,-1, config);
151  getController(leg,1).init(legName,"hinge",4,-1, config);
152  getController(leg,2).init(legName,"hinge",3,-1, config);
153  getController(leg,3).init(legName,"hinge",2,-1, config);
154  getController(leg,4).init(legName,"hinge",1,+1, config);
155  getController(leg,5).init(legName,"hinge",0,-1, config);
156  }else{
157  getController(leg,0).init(legName,"hinge",5,-1, config);
158  getController(leg,1).init(legName,"hinge",4,+1, config);
159  getController(leg,2).init(legName,"hinge",3,+1, config);
160  getController(leg,3).init(legName,"hinge",2,-1, config);
161  getController(leg,4).init(legName,"hinge",1,+1, config);
162  getController(leg,5).init(legName,"hinge",0,+1, config);
163  }
164  }
165  }
166 
167  if (flags.actTorso){
168  int torso = PART_TORSO;
169  const char *torsoName = "torso";
170  getController(torso,0).init(torsoName,"hinge",2,+1, config);
171  getController(torso,1).init(torsoName,"hinge",1,+1, config);
172  getController(torso,2).init(torsoName,"hinge",0,-1, config);
173  }
174 }
175 
177  return getController(part, axis);
178 }
Abstract class for mapping from "physical" (simulated) joints in the model to control joints in the I...
Definition: LogicalJoint.h:33
Convenience class for mapping from physical ODE joints in the model to control joints in the ICUB spe...
void init(const char *unit, const char *type, int index, int sign, RobotConfig &conf)
Initialize a regular control unit.
OdeLogicalJoint * nest(int len)
Create an array of nested control units.
virtual RobotFlags & getFlags()=0
bool actTorso
Definition: RobotConfig.h:31
bool actRArm
Definition: RobotConfig.h:31
bool actLArm
Definition: RobotConfig.h:31
bool actRHand
Definition: RobotConfig.h:31
bool valid
Definition: RobotConfig.h:30
bool actLHand
Definition: RobotConfig.h:31
bool actHead
Definition: RobotConfig.h:31
bool actLegs
Definition: RobotConfig.h:31
virtual LogicalJoint & control(int part, int axis)
Access the ODE control for a logical joint, based on part and axis number.
iCubLogicalJoints(RobotConfig &config)
Constructor.
#define PART_ARM_RIGHT
#define PART_ARM_LEFT
#define PART_TORSO
#define PART_HEAD_RAW
#define PART_LEG_RIGHT
#define PART_HEAD
#define PART_LEG_LEFT
Q15 sub(Q15 &a, Q15 &b)
Definition: strain.cpp:1222