1 #include "MinitaurSetup.h"
2 #include "b3RobotSimulatorClientAPI_NoGUI.h"
3
4 #include "Bullet3Common/b3HashMap.h"
5
6 struct MinitaurSetupInternalData
7 {
8 int m_quadrupedUniqueId;
9
MinitaurSetupInternalDataMinitaurSetupInternalData10 MinitaurSetupInternalData()
11 : m_quadrupedUniqueId(-1)
12 {
13 }
14
15 b3HashMap<b3HashString, int> m_jointNameToId;
16 };
17
MinitaurSetup()18 MinitaurSetup::MinitaurSetup()
19 {
20 m_data = new MinitaurSetupInternalData();
21 }
22
~MinitaurSetup()23 MinitaurSetup::~MinitaurSetup()
24 {
25 delete m_data;
26 }
27
setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI * sim,const char * motorName,double desiredAngle,double maxTorque,double kp,double kd)28 void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI_NoGUI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd)
29 {
30 b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
31 controlArgs.m_maxTorqueValue = maxTorque;
32 controlArgs.m_kd = kd;
33 controlArgs.m_kp = kp;
34 controlArgs.m_targetPosition = desiredAngle;
35 sim->setJointMotorControl(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorName], controlArgs);
36 }
37
38 //pick exactly 1 configuration of the following
39 #define MINITAUR_RAINBOWDASH_V1
40 //#define MINITAUR_RAINBOWDASH_V0
41 //#define MINITAUR_V0
42
43 #if defined(MINITAUR_RAINBOWDASH_V1)
44 #define MINITAUR_HAS_DEFORMABLE_BRACKETS
45 static const char* minitaurURDF = "quadruped/minitaur_rainbow_dash_v1.urdf";
46
47 static const char* kneeNames[] = {
48 "knee_front_leftL_joint", //1
49 "knee_front_leftR_joint", //3
50 "knee_back_leftL_joint", //5
51 "knee_back_leftR_joint", //7
52 "knee_front_rightL_joint", //9
53 "knee_back_rightL_joint", //10
54 "knee_back_rightR_joint", //13
55 "knee_front_rightR_joint", //15
56 };
57
58 static const char* motorNames[] = {
59 "motor_front_leftL_joint", //0
60 "knee_front_leftL_joint", //1
61 "motor_front_leftR_joint", //2
62 "knee_front_leftR_joint", //3
63 "motor_back_leftL_joint", //4
64 "knee_back_leftL_joint", //5
65 "motor_back_leftR_joint", //6
66 "knee_back_leftR_joint", //7
67 "motor_front_rightL_joint", //8
68 "knee_front_rightL_joint", //9
69 "knee_back_rightL_joint", //10
70 "motor_back_rightL_joint", //11
71 "motor_back_rightR_joint", //12
72 "knee_back_rightR_joint", //13
73 "motor_front_rightR_joint", //14
74 "knee_front_rightR_joint", //15
75 };
76
77 static const char* bracketNames[] = {
78 "motor_front_rightR_bracket_joint",
79 "motor_front_leftL_bracket_joint",
80 "motor_back_rightR_bracket_joint",
81 "motor_back_leftL_bracket_joint",
82 };
83
84 static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.0045, 0.088);
85 static btVector3 KNEE_CONSTRAINT_POINT_SHORT = btVector3(0, 0.0045, 0.100);
86 #elif defined(MINITAUR_RAINBOWDASH_V0)
87 static const char* minitaurURDF = "quadruped/minitaur_rainbow_dash.urdf";
88
89 static const char* kneeNames[] = {
90 "knee_front_leftL_joint", //1
91 "knee_front_leftR_joint", //3
92 "knee_back_leftL_joint", //5
93 "knee_back_leftR_joint", //7
94 "knee_front_rightL_joint", //9
95 "knee_back_rightL_joint", //10
96 "knee_back_rightR_joint", //13
97 "knee_front_rightR_joint", //15
98 };
99
100 static const char* motorNames[] = {
101 "motor_front_leftL_joint", //0
102 "knee_front_leftL_joint", //1
103 "motor_front_leftR_joint", //2
104 "knee_front_leftR_joint", //3
105 "motor_back_leftL_joint", //4
106 "knee_back_leftL_joint", //5
107 "motor_back_leftR_joint", //6
108 "knee_back_leftR_joint", //7
109 "motor_front_rightL_joint", //8
110 "knee_front_rightL_joint", //9
111 "knee_back_rightL_joint", //10
112 "motor_back_rightL_joint", //11
113 "motor_back_rightR_joint", //12
114 "knee_back_rightR_joint", //13
115 "motor_front_rightR_joint", //14
116 "knee_front_rightR_joint", //15
117 };
118 static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.0045, 0.088);
119 static btVector3 KNEE_CONSTRAINT_POINT_SHORT = btVector3(0, 0.0045, 0.100);
120 #elif defined(MINITAUR_V0)
121 static const char* minitaurURDF = "quadruped/minitaur.urdf";
122
123 static const char* kneeNames[] = {
124 "knee_front_leftL_link",
125 "knee_front_leftR_link",
126 "knee_back_leftL_link",
127 "knee_back_leftR_link",
128 "knee_front_rightL_link",
129 "knee_back_rightL_link",
130 "knee_back_rightR_link",
131 "knee_front_rightR_link",
132 };
133
134 static const char* motorNames[] = {
135 "motor_front_leftL_joint",
136 "knee_front_leftL_link",
137 "motor_front_leftR_joint",
138 "knee_front_leftR_link",
139 "motor_back_leftL_joint",
140 "knee_back_leftL_link",
141 "motor_back_leftR_joint",
142 "knee_back_leftR_link",
143 "motor_front_rightL_joint",
144 "knee_front_rightL_link",
145 "knee_back_rightL_link",
146 "motor_back_rightL_joint",
147 "motor_back_rightR_joint",
148 "knee_back_rightR_link",
149 "motor_front_rightR_joint",
150 "knee_front_rightR_link",
151 };
152 static btVector3 KNEE_CONSTRAINT_POINT_LONG = btVector3(0, 0.005, 0.2);
153 static btVector3 KNEE_CONSTRAINT_POINT_SHORT = btVector3(0, 0.01, 0.2);
154 #endif
155
resetPose(class b3RobotSimulatorClientAPI_NoGUI * sim)156 void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI_NoGUI* sim)
157 {
158 //release all motors
159 int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
160 for (int i = 0; i < numJoints; i++)
161 {
162 b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
163 controlArgs.m_maxTorqueValue = 0;
164 sim->setJointMotorControl(m_data->m_quadrupedUniqueId, i, controlArgs);
165 }
166
167 b3Scalar startAngle = B3_HALF_PI;
168 b3Scalar upperLegLength = 11.5;
169 b3Scalar lowerLegLength = 20;
170 b3Scalar kneeAngle = B3_PI + b3Acos(upperLegLength / lowerLegLength);
171
172 b3Scalar motorDirs[8] = {-1, -1, -1, -1, 1, 1, 1, 1};
173 b3JointInfo jointInfo;
174 jointInfo.m_jointType = ePoint2PointType;
175 //left front leg
176 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[0]], motorDirs[0] * startAngle);
177 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[0]], motorDirs[0] * kneeAngle);
178 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[2]], motorDirs[1] * startAngle);
179 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[1]], motorDirs[1] * kneeAngle);
180
181 jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0];
182 jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1];
183 jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
184 jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0];
185 jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1];
186 jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
187
188 //jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0]; jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1]; jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
189 //jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0]; jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1]; jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
190 sim->createConstraint(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[1]],
191 m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[0]], &jointInfo);
192 setDesiredMotorAngle(sim, motorNames[0], motorDirs[0] * startAngle);
193 setDesiredMotorAngle(sim, motorNames[2], motorDirs[1] * startAngle);
194
195 //left back leg
196 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[4]], motorDirs[2] * startAngle);
197 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[2]], motorDirs[2] * kneeAngle);
198 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[6]], motorDirs[3] * startAngle);
199 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[3]], motorDirs[3] * kneeAngle);
200 jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0];
201 jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1];
202 jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
203 jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0];
204 jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1];
205 jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
206
207 //jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0]; jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1]; jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
208 //jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0]; jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1]; jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
209 sim->createConstraint(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[3]],
210 m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[2]], &jointInfo);
211 setDesiredMotorAngle(sim, motorNames[4], motorDirs[2] * startAngle);
212 setDesiredMotorAngle(sim, motorNames[6], motorDirs[3] * startAngle);
213
214 //right front leg
215 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[8]], motorDirs[4] * startAngle);
216 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[4]], motorDirs[4] * kneeAngle);
217 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[14]], motorDirs[5] * startAngle);
218 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[7]], motorDirs[5] * kneeAngle);
219
220 jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0];
221 jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1];
222 jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
223 jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0];
224 jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1];
225 jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
226 sim->createConstraint(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[7]],
227 m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[4]], &jointInfo);
228 setDesiredMotorAngle(sim, motorNames[8], motorDirs[4] * startAngle);
229 setDesiredMotorAngle(sim, motorNames[14], motorDirs[5] * startAngle);
230
231 //right back leg
232 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[11]], motorDirs[6] * startAngle);
233 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[5]], motorDirs[6] * kneeAngle);
234 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[motorNames[12]], motorDirs[7] * startAngle);
235 sim->resetJointState(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[6]], motorDirs[7] * kneeAngle);
236
237 jointInfo.m_parentFrame[0] = KNEE_CONSTRAINT_POINT_LONG[0];
238 jointInfo.m_parentFrame[1] = KNEE_CONSTRAINT_POINT_LONG[1];
239 jointInfo.m_parentFrame[2] = KNEE_CONSTRAINT_POINT_LONG[2];
240 jointInfo.m_childFrame[0] = KNEE_CONSTRAINT_POINT_SHORT[0];
241 jointInfo.m_childFrame[1] = KNEE_CONSTRAINT_POINT_SHORT[1];
242 jointInfo.m_childFrame[2] = KNEE_CONSTRAINT_POINT_SHORT[2];
243 sim->createConstraint(m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[6]],
244 m_data->m_quadrupedUniqueId, *m_data->m_jointNameToId[kneeNames[5]], &jointInfo);
245 setDesiredMotorAngle(sim, motorNames[11], motorDirs[6] * startAngle);
246 setDesiredMotorAngle(sim, motorNames[12], motorDirs[7] * startAngle);
247
248 #ifdef MINITAUR_HAS_DEFORMABLE_BRACKETS
249 b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
250 controlArgs.m_maxTorqueValue = 6;
251 controlArgs.m_kd = 1;
252 controlArgs.m_kp = 0;
253 controlArgs.m_targetPosition = 0;
254 for (int i = 0; i < 4; i++)
255 {
256 const char* bracketName = bracketNames[i];
257 int* bracketId = m_data->m_jointNameToId[bracketName];
258 sim->setJointMotorControl(m_data->m_quadrupedUniqueId, *bracketId, controlArgs);
259 }
260
261 #endif
262 }
263
setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI * sim,const btVector3 & startPos,const btQuaternion & startOrn)264 int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI_NoGUI* sim, const btVector3& startPos, const btQuaternion& startOrn)
265 {
266 b3RobotSimulatorLoadUrdfFileArgs args;
267 args.m_startPosition = startPos;
268 args.m_startOrientation = startOrn;
269
270 m_data->m_quadrupedUniqueId = sim->loadURDF(minitaurURDF, args);
271
272 int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
273 for (int i = 0; i < numJoints; i++)
274 {
275 b3JointInfo jointInfo;
276 sim->getJointInfo(m_data->m_quadrupedUniqueId, i, &jointInfo);
277 if (jointInfo.m_jointName[0])
278 {
279 m_data->m_jointNameToId.insert(jointInfo.m_jointName, i);
280 }
281 }
282
283 resetPose(sim);
284
285 return m_data->m_quadrupedUniqueId;
286 }
287