8000 Lab Robot Non-periodic Gaits, Forward Kinematics test · col-in-coding/robot-modeling@5908ca4 · GitHub
[go: up one dir, main page]
Skip to content 8000

Commit 5908ca4

Browse files
col-in-codingcol-in-coding
committed
Lab Robot Non-periodic Gaits, Forward Kinematics test
1 parent 88316fb commit 5908ca4

File tree

2 files changed

+182
-20
lines changed

2 files changed

+182
-20
lines changed

‎hardware_test/LabRobot/StableGait/QuadrupedRobot.cpp

Lines changed: 131 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11

22
#include <Arduino.h>
3+
#include <math.h>
34
#include "QuadrupedRobot.h"
45

56
/**
@@ -117,48 +118,165 @@ void QuadrupedRobot::servo_move(float *angs_ptr)
117118
{
118119
timenow = millis();
119120
joint_angs_now[i] += joint_degs_speed[i] * (timenow - servo_time[i]);
121+
servo_time[i] = timenow; // update servo time
120122
}
121-
servo_write_angs(joint_angs_now);
123+
servo_write_angs(joint_angs_now, false);
122124
timenow = millis();
123125
}
124126
// last run to assure that all servo has been desired position
125127
for (size_t i = 0; i < DOF; i++)
126128
{
127-
servo_write_angs(angs_ptr);
129+
servo_write_angs(angs_ptr, false);
128130
}
129-
130-
131131
}
132132

133133
/**
134134
* Servo Write Current Angles
135135
* Current Angles is in Callibrated Coordinate,
136136
* which should be transfered to real angle degrees before using.
137137
*/
138-
void QuadrupedRobot::servo_write_angs(float *angs_ptr)
138+
void QuadrupedRobot::servo_write_angs(float *angs_ptr, bool is_in_degrees)
139139
{
140140
float real_ang{};
141141
float pulsewidth{};
142+
float ang_in_degree{};
142143
for (size_t i = 0; i < DOF; i++)
143144
{
144-
real_ang = init_servo_deg[i] + servo_dir[i] * angs_ptr[i];
145+
if (is_in_degrees){
146+
ang_in_degree = angs_ptr[i];
147+
} else {
148+
ang_in_degree = angs_ptr[i] * 180 / M_PI;
149+
}
150+
real_ang = init_servo_deg[i] + servo_dir[i] * ang_in_degree;
145151
pulsewidth = 500 + real_ang / 90.0 * 1000;
146152
if (!servos_ptr[i]->attached())
147153
{
148154
servos_ptr[i]->attach(servo_pins[i], SERVOMIN, SERVOMAX);
149155
}
150156
servos_ptr[i]->writeMicroseconds(pulsewidth);
151157
// update current angles buffer
152-
joint_angs_pre[i] = angs_ptr[i];
158+
if (is_in_degrees) {
159+
joint_angs_pre[i] = angs_ptr[i] * M_PI / 180;
160+
} else {
161+
joint_angs_pre[i] = angs_ptr[i];
162+
}
163+
}
164+
}
165+
166+
/**
167+
* Moving body by moving the feet in opposite direction
168+
* Body Frame xyz
169+
*/
170+
void QuadrupedRobot::body_xyz(float x, float y, float z)
171+
{
172+
for (size_t i = 0; i < 4; i++)
173+
{
174+
foot_pos[3 * i] = -x;
175+
foot_pos[3 * i + 1] = -y;
176+
foot_pos[3 * i + 2] = -z;
153177
}
178+
inverse_kinematics();
179+
servo_move(joint_angs_new);
180+
}
181+
182+
/**
183+
* Calculate the joint angles by foot positions
184+
*/
185+
void QuadrupedRobot::inverse_kinematics()
186+
{
187+
// FL 0, 1, 2
188+
joint_angs_new[0] = gamma_left(foot_pos[1], foot_pos[2]); // HipX
189+
vlegs_len[0] = vleg_left(foot_pos[0], foot_pos[1], joint_angs_new[0]);
190+
joint_angs_new[2] = beta_front(vlegs_len[0]);
191+
joint_angs_new[1] = alfa_front(foot_pos[0], joint_angs_new[2], vlegs_len[0]);
192+
// FR 3, 4, 5
193+
joint_angs_new[3] = gamma_right(foot_pos[4], foot_pos[5]);
194+
vlegs_len[3] = vleg_right(foot_pos[3], foot_pos[4], joint_angs_new[3]);
195+
joint_angs_new[5] = beta_front(vlegs_len[3]);
196+
joint_angs_new[4] = alfa_front(foot_pos[3], joint_angs_new[5], vlegs_len[3]);
197+
// RR 6, 7, 8
198+
joint_angs_new[6] = gamma_right(foot_pos[7], foot_pos[8]);
199+
vlegs_len[6] = vleg_right(foot_pos[6], foot_pos[7], joint_angs_new[6]);
200+
joint_angs_new[8] = beta_rear(vlegs_len[6]);
201+
joint_angs_new[7] = alfa_rear(foot_pos[6], joint_angs_new[8], vlegs_len[6]);
202+
// RL 9, 10, 11
203+
joint_angs_new[9] = gamma_left(foot_pos[10], foot_pos[11]);
204+
vlegs_len[9] = vleg_left(foot_pos[9], foot_pos[10], joint_angs_new[9]);
205+
joint_angs_new[11] = beta_rear(vlegs_len[9]);
206+
joint_angs_new[10] = alfa_rear(foot_pos[9], joint_angs_new[11], vlegs_len[9]);
207+
}
208+
209+
// Gamma: Hip Z angle
210+
float QuadrupedRobot::gamma_left(float dy, float dz)
211+
{
212+
float res = atan((toe_out0 - dz) / (height0 - dy)) - gamma0;
213+
return res;
214+
}
215+
216+
float QuadrupedRobot::gamma_right(float dy, float dz)
217+
{
218+
float res = gamma0 - atan((toe_out0 + dz) / (height0 - dy));
219+
return res;
220+
}
221+
// virtual leg length
222+
float QuadrupedRobot::vleg_left(float dx, float dy, float gamma)
223+
{
224+
float res = sqrt(pow(vleg_len0 - (dy / cos(gamma0 + gamma)), 2) + pow(dx, 2));
225+
if (res > thigh + calf)
226+
res = thigh + calf;
227+
return res;
228+
}
229+
230+
float QuadrupedRobot::vleg_right(float dx, float dy, float gamma)
231+
{
232+
float res = sqrt(pow(vleg_len0 - (dy / cos(gamma0 - gamma)), 2) + pow(dx, 2));
233+
if (res > thigh + calf)
234+
res = thigh + calf;
235+
return res;
236+
}
237+
238+
float QuadrupedRobot::beta_front(float vleg_len)
239+
{
240+
float res = M_PI - acos(
241+
(thigh * thigh + calf * calf - vleg_len * vleg_len) /
242+
(2 * thigh * calf));
243+
return res;
244+
}
245+
246+
float QuadrupedRobot::beta_rear(float vleg_len)
247+
{
248+
float res = acos(
249+
(thigh * thigh + calf * calf - vleg_len * vleg_len) /
250+
(2 * thigh * calf)) - M_PI;
251+
return res;
252+
}
253+
254+
float QuadrupedRobot::alfa_front(float dx, float beta, float vleg_len)
255+
{
256+
float res = asin(dx / vleg_len);
257+
res -= acos(
258+
(thigh * thigh + vleg_len * vleg_len - calf * calf) /
259+
(2 * thigh * vleg_len)
260+
);
261+
return res;
262+
}
263+
264+
float QuadrupedRobot::alfa_rear(float dx, float beta, float vleg_len)
265+
{
266+
float res = asin(dx / vleg_len);
267+
res += acos(
268+
(thigh * thigh + vleg_len * vleg_len - calf * calf) /
269+
(2 * thigh * vleg_len)
270+
);
271+
return res;
154272
}
155273

156274
/**
157275
* Only used for adjusting legs
158276
*/
159277
void QuadrupedRobot::adjust()
160278
{
161-
servo_write_angs(adjust_angs);
279+
servo_write_angs(adjust_angs, true);
162280
clear_cmd();
163281
}
164282

@@ -168,7 +286,7 @@ void QuadrupedRobot::adjust()
168286
*/
169287
void QuadrupedRobot::bot_stand()
170288
{
171-
servo_write_angs(stand_angs);
289+
servo_write_angs(stand_angs, true);
172290
clear_cmd();
173291
}
174292

@@ -178,7 +296,7 @@ void QuadrupedRobot::bot_stand()
178296
*/
179297
void QuadrupedRobot::bot_rest()
180298
{
181-
servo_write_angs(rest_angs);
299+
servo_write_angs(rest_angs, true);
182300
clear_cmd();
183301
}
184302

@@ -188,7 +306,9 @@ void QuadrupedRobot::bot_rest()
188306
*/
189307
void QuadrupedRobot::bot_walk()
190308
{
191-
Serial.print("walk");
309+
Serial.println("walk");
310+
body_xyz(0, 0, 0);
311+
clear_cmd();
192312
}
193313

194314
QuadrupedRobot::~QuadrupedRobot()

‎hardware_test/LabRobot/StableGait/QuadrupedRobot.h

Lines changed: 51 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33

44
#include <Arduino.h>
55
#include <Servo.h>
6+
#include <math.h>
67

78
#define PT(s) Serial.print(s)
89
#define PTL(s) Serial.println(s)
@@ -31,27 +32,68 @@ class QuadrupedRobot
3132
6, 7, 8,
3233
9, 10, 11,
3334
12, 13, 14};
34-
// Time taken by each sequence, used in servo_write_angs()
35+
// Time taken by each sequence, used in servo_move()
3536
unsigned long timestep = 500;
37+
int steplen = 40;
3638

37-
// Servo rotation configs
38-
const float init_servo_deg[12]{35, 90, 180, 130, 83, 0, 35, 85, 180, 130, 83, 0};
39-
const int8_t servo_dir[12]{1, -1, -1, -1, 1, 1, -1, 1, 1, 1, -1, -1};
40-
41-
// float joint_angs_now[12]{}; //Current joint angles, in degrees
39+
/**
40+
* Servo rotation configs
41+
* Real servo coord to calibrated coord: X-roll, Y-yaw, Z-pitch
42+
*/
43+
const float init_servo_deg[12]{35, 90, 180, 125, 83, 0, 40, 85, 180, 127, 83, 0};
44+
const int8_t servo_dir[12]{-1, -1, -1, -1, 1, 1, -1, 1, 1, -1, -1, -1};
45+
const float toe_out0{30}; // outward distance of toe during stand, in mm
46+
const float dist_ag{30}; // distance between alfa and gamma axis, in mm
47+
const float thigh{107};
48+
const float calf{90};
49+
/**
50+
* vleg_len: virtual leg length (composite with 2 limbs)
51+
* alfa, gamma, beta is conresponding to angles on HipZ, Knee, HipX axises
52+
*/
53+
const float vleg_len0{160};
54+
const float height0{sqrt(pow(vleg_len0 + dist_ag, 2) - pow(toe_out0, 2))};
55+
const float alfa0{acos((thigh * thigh + vleg_len0 * vleg_len0 - calf * calf) / (2 * thigh * vleg_len0))};
56+
const float beta0{M_PI - acos((thigh * thigh + calf * calf - vleg_len0 * vleg_len0) / (2 * thigh * calf))};
57+
const float gamma0{asin(toe_out0 / (vleg_len0 + dist_ag))};
58+
/**
59+
* State Variables
60+
* angles in radiums
61+
* Foot position, order: FLxyz, FRxyz, RLxyz, RRxyz
62+
*/
4263
float joint_angs_pre[12]{}; //Previous joint angles
43-
44-
// Angles in callibrated coordinate
64+
float joint_angs_new[12]{};
65+
float vlegs_len[4]{};
66+
float foot_pos[12] {};
67+
/**
68+
* Configed States
69+
* Angles in callibrated coordinate
70+
*/
4571
const float stand_angs[12]{0, -30, 60, 0, -30, 60, 0, 30, -60, 0, 30, -60};
4672
const float rest_angs[12]{0, -55, 130, 0, -55, 130, 0, 55, -130, 0, 55, -130};
4773
const float adjust_angs[12] {};
4874

75+
//
76+
// X points to forward, Z points to upward
77+
78+
4979
void setup_servos();
5080
void shut_servos();
5181
void servo_move(float *angs_ptr);
52-
void servo_write_angs(float *angs_ptr);
82+
void servo_write_angs(float *angs_ptr, bool is_in_degrees);
5383
void clear_cmd();
5484

85+
void body_xyz(float x, float y, float z);
86+
87+
void inverse_kinematics();
88+
float gamma_left(float dy, float dz);
89+
float gamma_right(float dy, float dz);
90+
float vleg_left(float dx, float dy, float gamma);
91+
float vleg_right(float dx, float dy, float gamma);
92+
float beta_front(float vleg_len);
93+
float beta_rear(float vleg_len);
94+
float alfa_front(float dx, float beta, float vleg_len);
95+
float alfa_rear(float dx, float beta, float vleg_len);
96+
5597
public:
5698
~QuadrupedRobot();
5799
bool interrupt();

0 commit comments

Comments
 (0)
0