一、介绍
本篇主要介绍四足机器人腿部结构设计,以及在webots仿真环境中的简单控制。
目前主流的腿部结构有曲柄滑块(spot),曲柄摇杆(宇数科技的),电机直驱(anymal),其余的还有液压驱动等就不展开介绍了。下面简单介绍一下不同方案的优缺点。
- 在这么多种驱动方案中,电机直驱在控制上可以说是最为直接的,因为不需要额外计算传动比,但是也带来一个弊端,关节的最大扭矩完全由电机的最大输出扭矩决定。并且由于电机安装在机器人腿上,使得其运动惯量加大,不适合高速运动。
- 而曲柄滑块,曲柄摇杆等间接驱动的方案,可以使得腿部更加轻盈,实现更加高速的运动,缺点就在于,无法像直驱那样提供精准的控制,毕竟传动部件越多,对装配,控制的要求也越高。并且控制系统给出各关节控制力矩之后,还需要额外的去计算实际电机输出扭矩,增加了算法的负责程度。
总的来说,各种方案之间各有利弊,没有好坏之分,只有适不适用。而在本篇当中,将会介绍曲柄滑块方案,即通过丝杆电机带动滑块从而实现腿关节的运动。
二、丝杆传动计算
首先我们定义以下符号:
- Ph:丝杆导程
- Fa:推力
- T:电机扭矩
- μ:转化效率
- S:滑台位移
- qm:电机角度
对于位置关系,我们有:
对于速度关系:
对于加速度关系:
而关于动力学部分,我们通常计算其扭矩关系,根据能量守恒定律,有
因此,当推动滑块丝杆收到的扭矩为:
当电机带动丝杆转动,对滑块产生的推力为:
三、对心曲柄滑块
首先我们定义以下图中的符号
滑块位移与关节角度关系如下:
速度关系如下:
加速度关系如下:
力学关系(滑块推力与关节扭矩)如下:
综合上式,我们可以对丝杆-曲柄滑块进行运动学建模
首先根据几何关系,我们可以得出:
令 λ = R\L:
则:
综上,我们得到电机角度到足端位置的关系
四、偏置曲柄滑块(滑块轴线与关节位置不在同一水平线)
根据上图的几何关系,我们可以得出以下公式
又因为 sin4β/4 近似于0,因此代入cosβ,可对其进行近似处理:
综合上式,我们可以得到
如果我们不对cosβ进行近似,得到的精确解为:
通过实测,这精确解跟近似解差别几乎可以忽略,对我们的腿部控制可以说是没有影响的,但是计算量却大了不少,因此实际使用中还是更推荐使用近似值。
而对于速度和加速度,我们对上式求导可以得到以下公式:
接下来我们对其进行运动学测试,设计关节角度函数:
取qmax=−30,qmin=−130,得到以下图像:
取qmax=−80,qmin=−130,得到:
测试代码如下:
import numpy as np
import matplotlib.pyplot as plt
R = 0.05
L = 0.1
L1 = 0.2
L2 = 0.2
e = 0.03
q_offset = np.arcsin(e/L1)
Q = 160*np.pi/180 + q_offset - np.pi
s_max = np.sqrt((R+L)**2-e**2)
def test3():
num_steps = 100
q_max = -80 * np.pi / 180
q_min = -140 * np.pi / 180
T = np.linspace(0, 2 * np.pi, num_steps)
dt = 2*np.pi/num_steps
q_array = (q_max - q_min) / 2 * np.sin(T) + (q_min + q_max) / 2
q_dot_array = (q_max - q_min) / 2 * np.cos(T)
q_ddot_array = -(q_max - q_min) / 2 * np.sin(T)
v_pre = get_dis_dot(q_array[0], q_dot_array[0])
s_pre = get_dis3(q_array[0])
s_list = []
v_list = []
v1_list = []
a_list = []
a1_list = []
for i in range(num_steps):
s = get_dis3(q_array[i])
v = get_dis_dot(q_array[i], q_dot_array[i])
a = get_dis_ddot(q_array[i], q_dot_array[i], q_ddot_array[i])
v1 = (s-s_pre)/dt
a1 = (v-v_pre)/dt
s_list.append(s)
v_list.append(v)
v1_list.append(v1)
a_list.append(a)
a1_list.append(a1)
s_pre = s
v_pre = v
# print('\t', a)
# print('\t', a1)
# print('-' * 50)
colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd',
'#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf']
s_list = np.array(s_list)
v_list = np.array(v_list)
v1_list = np.array(v1_list)
a_list = np.array(a_list)
a1_list = np.array(a1_list)
plt.figure()
plt.subplot(321)
plt.plot(T, s_list, label='s', color=colors[0])
plt.legend()
# plt.axis('equal')
plt.grid()
plt.subplot(322)
plt.plot(T, q_array / np.pi * 180, label='q', color=colors[1])
plt.legend()
# plt.axis('equal')
plt.grid()
plt.subplot(312)
plt.plot(T, v_list, label='v', color=colors[2])
plt.plot(T, v_list, label='v-true', color=colors[3])
plt.legend()
# plt.axis('equal')
plt.grid()
plt.subplot(313)
plt.plot(T, a_list, label='a', color=colors[2])
plt.plot(T, a1_list, label='a-true', color=colors[3])
plt.legend()
# plt.axis('equal')
plt.grid()
plt.show()
import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 160*np.pi/180 + q_offset - np.pi s_max = np.sqrt((R+L)**2-e**2) def test3(): num_steps = 100 q_max = -80 * np.pi / 180 q_min = -140 * np.pi / 180 T = np.linspace(0, 2 * np.pi, num_steps) dt = 2*np.pi/num_steps q_array = (q_max - q_min) / 2 * np.sin(T) + (q_min + q_max) / 2 q_dot_array = (q_max - q_min) / 2 * np.cos(T) q_ddot_array = -(q_max - q_min) / 2 * np.sin(T) v_pre = get_dis_dot(q_array[0], q_dot_array[0]) s_pre = get_dis3(q_array[0]) s_list = [] v_list = [] v1_list = [] a_list = [] a1_list = [] for i in range(num_steps): s = get_dis3(q_array[i]) v = get_dis_dot(q_array[i], q_dot_array[i]) a = get_dis_ddot(q_array[i], q_dot_array[i], q_ddot_array[i]) v1 = (s-s_pre)/dt a1 = (v-v_pre)/dt s_list.append(s) v_list.append(v) v1_list.append(v1) a_list.append(a) a1_list.append(a1) s_pre = s v_pre = v # print('\t', a) # print('\t', a1) # print('-' * 50) colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf'] s_list = np.array(s_list) v_list = np.array(v_list) v1_list = np.array(v1_list) a_list = np.array(a_list) a1_list = np.array(a1_list) plt.figure() plt.subplot(321) plt.plot(T, s_list, label='s', color=colors[0]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(322) plt.plot(T, q_array / np.pi * 180, label='q', color=colors[1]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(312) plt.plot(T, v_list, label='v', color=colors[2]) plt.plot(T, v_list, label='v-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(313) plt.plot(T, a_list, label='a', color=colors[2]) plt.plot(T, a1_list, label='a-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.show()
复制
import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 160*np.pi/180 + q_offset - np.pi s_max = np.sqrt((R+L)**2-e**2) def test3(): num_steps = 100 q_max = -80 * np.pi / 180 q_min = -140 * np.pi / 180 T = np.linspace(0, 2 * np.pi, num_steps) dt = 2*np.pi/num_steps q_array = (q_max - q_min) / 2 * np.sin(T) + (q_min + q_max) / 2 q_dot_array = (q_max - q_min) / 2 * np.cos(T) q_ddot_array = -(q_max - q_min) / 2 * np.sin(T) v_pre = get_dis_dot(q_array[0], q_dot_array[0]) s_pre = get_dis3(q_array[0]) s_list = [] v_list = [] v1_list = [] a_list = [] a1_list = [] for i in range(num_steps): s = get_dis3(q_array[i]) v = get_dis_dot(q_array[i], q_dot_array[i]) a = get_dis_ddot(q_array[i], q_dot_array[i], q_ddot_array[i]) v1 = (s-s_pre)/dt a1 = (v-v_pre)/dt s_list.append(s) v_list.append(v) v1_list.append(v1) a_list.append(a) a1_list.append(a1) s_pre = s v_pre = v # print('\t', a) # print('\t', a1) # print('-' * 50) colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf'] s_list = np.array(s_list) v_list = np.array(v_list) v1_list = np.array(v1_list) a_list = np.array(a_list) a1_list = np.array(a1_list) plt.figure() plt.subplot(321) plt.plot(T, s_list, label='s', color=colors[0]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(322) plt.plot(T, q_array / np.pi * 180, label='q', color=colors[1]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(312) plt.plot(T, v_list, label='v', color=colors[2]) plt.plot(T, v_list, label='v-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(313) plt.plot(T, a_list, label='a', color=colors[2]) plt.plot(T, a1_list, label='a-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.show()
复制
复制
复制
import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 160*np.pi/180 + q_offset - np.pi s_max = np.sqrt((R+L)**2-e**2) def test3(): num_steps = 100 q_max = -80 * np.pi / 180 q_min = -140 * np.pi / 180 T = np.linspace(0, 2 * np.pi, num_steps) dt = 2*np.pi/num_steps q_array = (q_max - q_min) / 2 * np.sin(T) + (q_min + q_max) / 2 q_dot_array = (q_max - q_min) / 2 * np.cos(T) q_ddot_array = -(q_max - q_min) / 2 * np.sin(T) v_pre = get_dis_dot(q_array[0], q_dot_array[0]) s_pre = get_dis3(q_array[0]) s_list = [] v_list = [] v1_list = [] a_list = [] a1_list = [] for i in range(num_steps): s = get_dis3(q_array[i]) v = get_dis_dot(q_array[i], q_dot_array[i]) a = get_dis_ddot(q_array[i], q_dot_array[i], q_ddot_array[i]) v1 = (s-s_pre)/dt a1 = (v-v_pre)/dt s_list.append(s) v_list.append(v) v1_list.append(v1) a_list.append(a) a1_list.append(a1) s_pre = s v_pre = v # print('\t', a) # print('\t', a1) # print('-' * 50) colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf'] s_list = np.array(s_list) v_list = np.array(v_list) v1_list = np.array(v1_list) a_list = np.array(a_list) a1_list = np.array(a1_list) plt.figure() plt.subplot(321) plt.plot(T, s_list, label='s', color=colors[0]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(322) plt.plot(T, q_array / np.pi * 180, label='q', color=colors[1]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(312) plt.plot(T, v_list, label='v', color=colors[2]) plt.plot(T, v_list, label='v-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(313) plt.plot(T, a_list, label='a', color=colors[2]) plt.plot(T, a1_list, label='a-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.show()
复制
复制
复制
复制
复制
复制
复制
import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 160*np.pi/180 + q_offset - np.pi s_max = np.sqrt((R+L)**2-e**2) def test3(): num_steps = 100 q_max = -80 * np.pi / 180 q_min = -140 * np.pi / 180 T = np.linspace(0, 2 * np.pi, num_steps) dt = 2*np.pi/num_steps q_array = (q_max - q_min) / 2 * np.sin(T) + (q_min + q_max) / 2 q_dot_array = (q_max - q_min) / 2 * np.cos(T) q_ddot_array = -(q_max - q_min) / 2 * np.sin(T) v_pre = get_dis_dot(q_array[0], q_dot_array[0]) s_pre = get_dis3(q_array[0]) s_list = [] v_list = [] v1_list = [] a_list = [] a1_list = [] for i in range(num_steps): s = get_dis3(q_array[i]) v = get_dis_dot(q_array[i], q_dot_array[i]) a = get_dis_ddot(q_array[i], q_dot_array[i], q_ddot_array[i]) v1 = (s-s_pre)/dt a1 = (v-v_pre)/dt s_list.append(s) v_list.append(v) v1_list.append(v1) a_list.append(a) a1_list.append(a1) s_pre = s v_pre = v # print('\t', a) # print('\t', a1) # print('-' * 50) colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf'] s_list = np.array(s_list) v_list = np.array(v_list) v1_list = np.array(v1_list) a_list = np.array(a_list) a1_list = np.array(a1_list) plt.figure() plt.subplot(321) plt.plot(T, s_list, label='s', color=colors[0]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(322) plt.plot(T, q_array / np.pi * 180, label='q', color=colors[1]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(312) plt.plot(T, v_list, label='v', color=colors[2]) plt.plot(T, v_list, label='v-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(313) plt.plot(T, a_list, label='a', color=colors[2]) plt.plot(T, a1_list, label='a-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.show()
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 160*np.pi/180 + q_offset - np.pi s_max = np.sqrt((R+L)**2-e**2) def test3(): num_steps = 100 q_max = -80 * np.pi / 180 q_min = -140 * np.pi / 180 T = np.linspace(0, 2 * np.pi, num_steps) dt = 2*np.pi/num_steps q_array = (q_max - q_min) / 2 * np.sin(T) + (q_min + q_max) / 2 q_dot_array = (q_max - q_min) / 2 * np.cos(T) q_ddot_array = -(q_max - q_min) / 2 * np.sin(T) v_pre = get_dis_dot(q_array[0], q_dot_array[0]) s_pre = get_dis3(q_array[0]) s_list = [] v_list = [] v1_list = [] a_list = [] a1_list = [] for i in range(num_steps): s = get_dis3(q_array[i]) v = get_dis_dot(q_array[i], q_dot_array[i]) a = get_dis_ddot(q_array[i], q_dot_array[i], q_ddot_array[i]) v1 = (s-s_pre)/dt a1 = (v-v_pre)/dt s_list.append(s) v_list.append(v) v1_list.append(v1) a_list.append(a) a1_list.append(a1) s_pre = s v_pre = v # print('\t', a) # print('\t', a1) # print('-' * 50) colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf'] s_list = np.array(s_list) v_list = np.array(v_list) v1_list = np.array(v1_list) a_list = np.array(a_list) a1_list = np.array(a1_list) plt.figure() plt.subplot(321) plt.plot(T, s_list, label='s', color=colors[0]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(322) plt.plot(T, q_array / np.pi * 180, label='q', color=colors[1]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(312) plt.plot(T, v_list, label='v', color=colors[2]) plt.plot(T, v_list, label='v-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(313) plt.plot(T, a_list, label='a', color=colors[2]) plt.plot(T, a1_list, label='a-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.show()
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 160*np.pi/180 + q_offset - np.pi s_max = np.sqrt((R+L)**2-e**2) def test3(): num_steps = 100 q_max = -80 * np.pi / 180 q_min = -140 * np.pi / 180 T = np.linspace(0, 2 * np.pi, num_steps) dt = 2*np.pi/num_steps q_array = (q_max - q_min) / 2 * np.sin(T) + (q_min + q_max) / 2 q_dot_array = (q_max - q_min) / 2 * np.cos(T) q_ddot_array = -(q_max - q_min) / 2 * np.sin(T) v_pre = get_dis_dot(q_array[0], q_dot_array[0]) s_pre = get_dis3(q_array[0]) s_list = [] v_list = [] v1_list = [] a_list = [] a1_list = [] for i in range(num_steps): s = get_dis3(q_array[i]) v = get_dis_dot(q_array[i], q_dot_array[i]) a = get_dis_ddot(q_array[i], q_dot_array[i], q_ddot_array[i]) v1 = (s-s_pre)/dt a1 = (v-v_pre)/dt s_list.append(s) v_list.append(v) v1_list.append(v1) a_list.append(a) a1_list.append(a1) s_pre = s v_pre = v # print('\t', a) # print('\t', a1) # print('-' * 50) colors = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf'] s_list = np.array(s_list) v_list = np.array(v_list) v1_list = np.array(v1_list) a_list = np.array(a_list) a1_list = np.array(a1_list) plt.figure() plt.subplot(321) plt.plot(T, s_list, label='s', color=colors[0]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(322) plt.plot(T, q_array / np.pi * 180, label='q', color=colors[1]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(312) plt.plot(T, v_list, label='v', color=colors[2]) plt.plot(T, v_list, label='v-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.subplot(313) plt.plot(T, a_list, label='a', color=colors[2]) plt.plot(T, a1_list, label='a-true', color=colors[3]) plt.legend() # plt.axis('equal') plt.grid() plt.show()
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
四、仿真控制
我们利用webots进行仿真控制,控制器代码如下:
from controller import Robot
import numpy as np
import matplotlib.pyplot as plt
R = 0.05
L = 0.1
L1 = 0.2
L2 = 0.2
e = 0.03
q_offset = np.arcsin(e/L1)
Q = 150*np.pi/180 + q_offset
s_max = np.sqrt((R+L)**2-e**2)
robot = Robot()
TIME_STEP = int(robot.getBasicTimeStep())
motors = []
motor_names = ['hip motor',
'knee linear motor']
position_sensors = []
position_sensor_names = ['hip position sensor',
'knee position sensor',
'slider position sensor']
q_pre = np.array([0, 0])
def get_data():
global q_pre
q = [0, 0]
qd = [0, 0]
q[0] = position_sensors[0].getValue()
q[1] = position_sensors[1].getValue()
# q[2] = position_sensors[2].getValue()
qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP
qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP
# qd[2] = q[2] - q_pre[2]
q_pre = q + np.array([0, 0])
return np.array(q), np.array(qd)
# 逆运动学(二维)
def inverse_kinematic(x, y):
theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2))
theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y))
print('des knee angle:\t', theta2 * 180 / np.pi)
print('des hip angle:\t', theta1 * 180 / np.pi)
alpha = Q + theta2
# print('alpha:\t', alpha * 180 / np.pi)
s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L
return [theta1, s]
def forward_kinematic(angle1, angle2):
x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2)
z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2)
return np.array([x, z])
def jacobian(theta1, theta2):
J = np.zeros([2, 2])
J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2)
J[0, 1] = -L2 * np.cos(theta1 + theta2)
J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
J[1, 1] = L2 * np.sin(theta1 + theta2)
return J
def dynamic(force):
theta1 = position_sensors[0].getValue()
theta2 = position_sensors[1].getValue()
tau = np.matmul(jacobian(theta1, theta2).transpose(), force)
print('tau: ', tau)
alpha = Q + theta2
c_alpha = np.cos(alpha)
s_alpha = np.sin(alpha)
s_beta = (R * s_alpha - e) / L
c_beta = 1 - 0.5 * s_beta ** 2
m1 = R * (s_alpha*c_beta + c_alpha*s_beta)
P = tau[1] * c_beta / m1
return [tau[0], P]
def stand(height):
kp = np.array([[10, 0], [0, 1500]])
kd = np.array([[1, 0], [0, 500]])
p_des = np.array([0, -height])
p_dot_des = np.array([0, 0])
q, qd = get_data()
# print(q)
# print(qd)
p_current = forward_kinematic(q[0], q[1])
p_dot_current = np.matmul(jacobian(q[0], q[1]), qd)
print('p_dot_current:\t', p_dot_current)
print('p_current:\t', p_current)
force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current)
print('force:\t', force)
tau = dynamic(force)
print('command:\t', tau)
return tau
def init():
for i in range(len(motor_names)):
motors.append(robot.getDevice(motor_names[i]))
for i in range(len(position_sensor_names)):
position_sensors.append((robot.getDevice(position_sensor_names[i])))
position_sensors[i].enable(TIME_STEP)
# angle1, angle2 = inverse_kinematic(0, -0.5)
# motors[0].setPosition(angle1)
# motors[1].setPosition(angle2)
cnt = 0
init()
while robot.step(TIME_STEP) != -1:
# angle1, angle2 = inverse_kinematic(0, height)
# motors[0].setPosition(angle1)
# motors[1].setPosition(angle2)
# height = 0.075*np.sin(cnt) + 0.425
tau = stand(0.2)
# tau = [0, 100]
motors[0].setTorque(tau[0])
motors[1].setForce(tau[1])
hip_angle = position_sensors[0].getValue()
knee_angle = position_sensors[1].getValue()
slider_pos = position_sensors[2].getValue()
print('L_bar:\t', s_max)
# print('S:\t', angle2)
print('hip joint:\t', hip_angle*180/np.pi)
print('knee joint:\t', knee_angle*180/np.pi)
print('linear position:\t', slider_pos)
print('-'*50)
cnt += 0.1
from controller import Robot import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 150*np.pi/180 + q_offset s_max = np.sqrt((R+L)**2-e**2) robot = Robot() TIME_STEP = int(robot.getBasicTimeStep()) motors = [] motor_names = ['hip motor', 'knee linear motor'] position_sensors = [] position_sensor_names = ['hip position sensor', 'knee position sensor', 'slider position sensor'] q_pre = np.array([0, 0]) def get_data(): global q_pre q = [0, 0] qd = [0, 0] q[0] = position_sensors[0].getValue() q[1] = position_sensors[1].getValue() # q[2] = position_sensors[2].getValue() qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP # qd[2] = q[2] - q_pre[2] q_pre = q + np.array([0, 0]) return np.array(q), np.array(qd) # 逆运动学(二维) def inverse_kinematic(x, y): theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2)) theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y)) print('des knee angle:\t', theta2 * 180 / np.pi) print('des hip angle:\t', theta1 * 180 / np.pi) alpha = Q + theta2 # print('alpha:\t', alpha * 180 / np.pi) s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L return [theta1, s] def forward_kinematic(angle1, angle2): x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2) z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2) return np.array([x, z]) def jacobian(theta1, theta2): J = np.zeros([2, 2]) J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2) J[0, 1] = -L2 * np.cos(theta1 + theta2) J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) J[1, 1] = L2 * np.sin(theta1 + theta2) return J def dynamic(force): theta1 = position_sensors[0].getValue() theta2 = position_sensors[1].getValue() tau = np.matmul(jacobian(theta1, theta2).transpose(), force) print('tau: ', tau) alpha = Q + theta2 c_alpha = np.cos(alpha) s_alpha = np.sin(alpha) s_beta = (R * s_alpha - e) / L c_beta = 1 - 0.5 * s_beta ** 2 m1 = R * (s_alpha*c_beta + c_alpha*s_beta) P = tau[1] * c_beta / m1 return [tau[0], P] def stand(height): kp = np.array([[10, 0], [0, 1500]]) kd = np.array([[1, 0], [0, 500]]) p_des = np.array([0, -height]) p_dot_des = np.array([0, 0]) q, qd = get_data() # print(q) # print(qd) p_current = forward_kinematic(q[0], q[1]) p_dot_current = np.matmul(jacobian(q[0], q[1]), qd) print('p_dot_current:\t', p_dot_current) print('p_current:\t', p_current) force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current) print('force:\t', force) tau = dynamic(force) print('command:\t', tau) return tau def init(): for i in range(len(motor_names)): motors.append(robot.getDevice(motor_names[i])) for i in range(len(position_sensor_names)): position_sensors.append((robot.getDevice(position_sensor_names[i]))) position_sensors[i].enable(TIME_STEP) # angle1, angle2 = inverse_kinematic(0, -0.5) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) cnt = 0 init() while robot.step(TIME_STEP) != -1: # angle1, angle2 = inverse_kinematic(0, height) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) # height = 0.075*np.sin(cnt) + 0.425 tau = stand(0.2) # tau = [0, 100] motors[0].setTorque(tau[0]) motors[1].setForce(tau[1]) hip_angle = position_sensors[0].getValue() knee_angle = position_sensors[1].getValue() slider_pos = position_sensors[2].getValue() print('L_bar:\t', s_max) # print('S:\t', angle2) print('hip joint:\t', hip_angle*180/np.pi) print('knee joint:\t', knee_angle*180/np.pi) print('linear position:\t', slider_pos) print('-'*50) cnt += 0.1
复制
from controller import Robot import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 150*np.pi/180 + q_offset s_max = np.sqrt((R+L)**2-e**2) robot = Robot() TIME_STEP = int(robot.getBasicTimeStep()) motors = [] motor_names = ['hip motor', 'knee linear motor'] position_sensors = [] position_sensor_names = ['hip position sensor', 'knee position sensor', 'slider position sensor'] q_pre = np.array([0, 0]) def get_data(): global q_pre q = [0, 0] qd = [0, 0] q[0] = position_sensors[0].getValue() q[1] = position_sensors[1].getValue() # q[2] = position_sensors[2].getValue() qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP # qd[2] = q[2] - q_pre[2] q_pre = q + np.array([0, 0]) return np.array(q), np.array(qd) # 逆运动学(二维) def inverse_kinematic(x, y): theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2)) theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y)) print('des knee angle:\t', theta2 * 180 / np.pi) print('des hip angle:\t', theta1 * 180 / np.pi) alpha = Q + theta2 # print('alpha:\t', alpha * 180 / np.pi) s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L return [theta1, s] def forward_kinematic(angle1, angle2): x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2) z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2) return np.array([x, z]) def jacobian(theta1, theta2): J = np.zeros([2, 2]) J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2) J[0, 1] = -L2 * np.cos(theta1 + theta2) J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) J[1, 1] = L2 * np.sin(theta1 + theta2) return J def dynamic(force): theta1 = position_sensors[0].getValue() theta2 = position_sensors[1].getValue() tau = np.matmul(jacobian(theta1, theta2).transpose(), force) print('tau: ', tau) alpha = Q + theta2 c_alpha = np.cos(alpha) s_alpha = np.sin(alpha) s_beta = (R * s_alpha - e) / L c_beta = 1 - 0.5 * s_beta ** 2 m1 = R * (s_alpha*c_beta + c_alpha*s_beta) P = tau[1] * c_beta / m1 return [tau[0], P] def stand(height): kp = np.array([[10, 0], [0, 1500]]) kd = np.array([[1, 0], [0, 500]]) p_des = np.array([0, -height]) p_dot_des = np.array([0, 0]) q, qd = get_data() # print(q) # print(qd) p_current = forward_kinematic(q[0], q[1]) p_dot_current = np.matmul(jacobian(q[0], q[1]), qd) print('p_dot_current:\t', p_dot_current) print('p_current:\t', p_current) force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current) print('force:\t', force) tau = dynamic(force) print('command:\t', tau) return tau def init(): for i in range(len(motor_names)): motors.append(robot.getDevice(motor_names[i])) for i in range(len(position_sensor_names)): position_sensors.append((robot.getDevice(position_sensor_names[i]))) position_sensors[i].enable(TIME_STEP) # angle1, angle2 = inverse_kinematic(0, -0.5) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) cnt = 0 init() while robot.step(TIME_STEP) != -1: # angle1, angle2 = inverse_kinematic(0, height) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) # height = 0.075*np.sin(cnt) + 0.425 tau = stand(0.2) # tau = [0, 100] motors[0].setTorque(tau[0]) motors[1].setForce(tau[1]) hip_angle = position_sensors[0].getValue() knee_angle = position_sensors[1].getValue() slider_pos = position_sensors[2].getValue() print('L_bar:\t', s_max) # print('S:\t', angle2) print('hip joint:\t', hip_angle*180/np.pi) print('knee joint:\t', knee_angle*180/np.pi) print('linear position:\t', slider_pos) print('-'*50) cnt += 0.1
复制
复制
复制
from controller import Robot import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 150*np.pi/180 + q_offset s_max = np.sqrt((R+L)**2-e**2) robot = Robot() TIME_STEP = int(robot.getBasicTimeStep()) motors = [] motor_names = ['hip motor', 'knee linear motor'] position_sensors = [] position_sensor_names = ['hip position sensor', 'knee position sensor', 'slider position sensor'] q_pre = np.array([0, 0]) def get_data(): global q_pre q = [0, 0] qd = [0, 0] q[0] = position_sensors[0].getValue() q[1] = position_sensors[1].getValue() # q[2] = position_sensors[2].getValue() qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP # qd[2] = q[2] - q_pre[2] q_pre = q + np.array([0, 0]) return np.array(q), np.array(qd) # 逆运动学(二维) def inverse_kinematic(x, y): theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2)) theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y)) print('des knee angle:\t', theta2 * 180 / np.pi) print('des hip angle:\t', theta1 * 180 / np.pi) alpha = Q + theta2 # print('alpha:\t', alpha * 180 / np.pi) s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L return [theta1, s] def forward_kinematic(angle1, angle2): x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2) z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2) return np.array([x, z]) def jacobian(theta1, theta2): J = np.zeros([2, 2]) J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2) J[0, 1] = -L2 * np.cos(theta1 + theta2) J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) J[1, 1] = L2 * np.sin(theta1 + theta2) return J def dynamic(force): theta1 = position_sensors[0].getValue() theta2 = position_sensors[1].getValue() tau = np.matmul(jacobian(theta1, theta2).transpose(), force) print('tau: ', tau) alpha = Q + theta2 c_alpha = np.cos(alpha) s_alpha = np.sin(alpha) s_beta = (R * s_alpha - e) / L c_beta = 1 - 0.5 * s_beta ** 2 m1 = R * (s_alpha*c_beta + c_alpha*s_beta) P = tau[1] * c_beta / m1 return [tau[0], P] def stand(height): kp = np.array([[10, 0], [0, 1500]]) kd = np.array([[1, 0], [0, 500]]) p_des = np.array([0, -height]) p_dot_des = np.array([0, 0]) q, qd = get_data() # print(q) # print(qd) p_current = forward_kinematic(q[0], q[1]) p_dot_current = np.matmul(jacobian(q[0], q[1]), qd) print('p_dot_current:\t', p_dot_current) print('p_current:\t', p_current) force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current) print('force:\t', force) tau = dynamic(force) print('command:\t', tau) return tau def init(): for i in range(len(motor_names)): motors.append(robot.getDevice(motor_names[i])) for i in range(len(position_sensor_names)): position_sensors.append((robot.getDevice(position_sensor_names[i]))) position_sensors[i].enable(TIME_STEP) # angle1, angle2 = inverse_kinematic(0, -0.5) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) cnt = 0 init() while robot.step(TIME_STEP) != -1: # angle1, angle2 = inverse_kinematic(0, height) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) # height = 0.075*np.sin(cnt) + 0.425 tau = stand(0.2) # tau = [0, 100] motors[0].setTorque(tau[0]) motors[1].setForce(tau[1]) hip_angle = position_sensors[0].getValue() knee_angle = position_sensors[1].getValue() slider_pos = position_sensors[2].getValue() print('L_bar:\t', s_max) # print('S:\t', angle2) print('hip joint:\t', hip_angle*180/np.pi) print('knee joint:\t', knee_angle*180/np.pi) print('linear position:\t', slider_pos) print('-'*50) cnt += 0.1
复制
复制
复制
复制
复制
复制
复制
from controller import Robot import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 150*np.pi/180 + q_offset s_max = np.sqrt((R+L)**2-e**2) robot = Robot() TIME_STEP = int(robot.getBasicTimeStep()) motors = [] motor_names = ['hip motor', 'knee linear motor'] position_sensors = [] position_sensor_names = ['hip position sensor', 'knee position sensor', 'slider position sensor'] q_pre = np.array([0, 0]) def get_data(): global q_pre q = [0, 0] qd = [0, 0] q[0] = position_sensors[0].getValue() q[1] = position_sensors[1].getValue() # q[2] = position_sensors[2].getValue() qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP # qd[2] = q[2] - q_pre[2] q_pre = q + np.array([0, 0]) return np.array(q), np.array(qd) # 逆运动学(二维) def inverse_kinematic(x, y): theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2)) theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y)) print('des knee angle:\t', theta2 * 180 / np.pi) print('des hip angle:\t', theta1 * 180 / np.pi) alpha = Q + theta2 # print('alpha:\t', alpha * 180 / np.pi) s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L return [theta1, s] def forward_kinematic(angle1, angle2): x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2) z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2) return np.array([x, z]) def jacobian(theta1, theta2): J = np.zeros([2, 2]) J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2) J[0, 1] = -L2 * np.cos(theta1 + theta2) J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) J[1, 1] = L2 * np.sin(theta1 + theta2) return J def dynamic(force): theta1 = position_sensors[0].getValue() theta2 = position_sensors[1].getValue() tau = np.matmul(jacobian(theta1, theta2).transpose(), force) print('tau: ', tau) alpha = Q + theta2 c_alpha = np.cos(alpha) s_alpha = np.sin(alpha) s_beta = (R * s_alpha - e) / L c_beta = 1 - 0.5 * s_beta ** 2 m1 = R * (s_alpha*c_beta + c_alpha*s_beta) P = tau[1] * c_beta / m1 return [tau[0], P] def stand(height): kp = np.array([[10, 0], [0, 1500]]) kd = np.array([[1, 0], [0, 500]]) p_des = np.array([0, -height]) p_dot_des = np.array([0, 0]) q, qd = get_data() # print(q) # print(qd) p_current = forward_kinematic(q[0], q[1]) p_dot_current = np.matmul(jacobian(q[0], q[1]), qd) print('p_dot_current:\t', p_dot_current) print('p_current:\t', p_current) force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current) print('force:\t', force) tau = dynamic(force) print('command:\t', tau) return tau def init(): for i in range(len(motor_names)): motors.append(robot.getDevice(motor_names[i])) for i in range(len(position_sensor_names)): position_sensors.append((robot.getDevice(position_sensor_names[i]))) position_sensors[i].enable(TIME_STEP) # angle1, angle2 = inverse_kinematic(0, -0.5) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) cnt = 0 init() while robot.step(TIME_STEP) != -1: # angle1, angle2 = inverse_kinematic(0, height) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) # height = 0.075*np.sin(cnt) + 0.425 tau = stand(0.2) # tau = [0, 100] motors[0].setTorque(tau[0]) motors[1].setForce(tau[1]) hip_angle = position_sensors[0].getValue() knee_angle = position_sensors[1].getValue() slider_pos = position_sensors[2].getValue() print('L_bar:\t', s_max) # print('S:\t', angle2) print('hip joint:\t', hip_angle*180/np.pi) print('knee joint:\t', knee_angle*180/np.pi) print('linear position:\t', slider_pos) print('-'*50) cnt += 0.1
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
from controller import Robot import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 150*np.pi/180 + q_offset s_max = np.sqrt((R+L)**2-e**2) robot = Robot() TIME_STEP = int(robot.getBasicTimeStep()) motors = [] motor_names = ['hip motor', 'knee linear motor'] position_sensors = [] position_sensor_names = ['hip position sensor', 'knee position sensor', 'slider position sensor'] q_pre = np.array([0, 0]) def get_data(): global q_pre q = [0, 0] qd = [0, 0] q[0] = position_sensors[0].getValue() q[1] = position_sensors[1].getValue() # q[2] = position_sensors[2].getValue() qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP # qd[2] = q[2] - q_pre[2] q_pre = q + np.array([0, 0]) return np.array(q), np.array(qd) # 逆运动学(二维) def inverse_kinematic(x, y): theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2)) theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y)) print('des knee angle:\t', theta2 * 180 / np.pi) print('des hip angle:\t', theta1 * 180 / np.pi) alpha = Q + theta2 # print('alpha:\t', alpha * 180 / np.pi) s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L return [theta1, s] def forward_kinematic(angle1, angle2): x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2) z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2) return np.array([x, z]) def jacobian(theta1, theta2): J = np.zeros([2, 2]) J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2) J[0, 1] = -L2 * np.cos(theta1 + theta2) J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) J[1, 1] = L2 * np.sin(theta1 + theta2) return J def dynamic(force): theta1 = position_sensors[0].getValue() theta2 = position_sensors[1].getValue() tau = np.matmul(jacobian(theta1, theta2).transpose(), force) print('tau: ', tau) alpha = Q + theta2 c_alpha = np.cos(alpha) s_alpha = np.sin(alpha) s_beta = (R * s_alpha - e) / L c_beta = 1 - 0.5 * s_beta ** 2 m1 = R * (s_alpha*c_beta + c_alpha*s_beta) P = tau[1] * c_beta / m1 return [tau[0], P] def stand(height): kp = np.array([[10, 0], [0, 1500]]) kd = np.array([[1, 0], [0, 500]]) p_des = np.array([0, -height]) p_dot_des = np.array([0, 0]) q, qd = get_data() # print(q) # print(qd) p_current = forward_kinematic(q[0], q[1]) p_dot_current = np.matmul(jacobian(q[0], q[1]), qd) print('p_dot_current:\t', p_dot_current) print('p_current:\t', p_current) force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current) print('force:\t', force) tau = dynamic(force) print('command:\t', tau) return tau def init(): for i in range(len(motor_names)): motors.append(robot.getDevice(motor_names[i])) for i in range(len(position_sensor_names)): position_sensors.append((robot.getDevice(position_sensor_names[i]))) position_sensors[i].enable(TIME_STEP) # angle1, angle2 = inverse_kinematic(0, -0.5) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) cnt = 0 init() while robot.step(TIME_STEP) != -1: # angle1, angle2 = inverse_kinematic(0, height) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) # height = 0.075*np.sin(cnt) + 0.425 tau = stand(0.2) # tau = [0, 100] motors[0].setTorque(tau[0]) motors[1].setForce(tau[1]) hip_angle = position_sensors[0].getValue() knee_angle = position_sensors[1].getValue() slider_pos = position_sensors[2].getValue() print('L_bar:\t', s_max) # print('S:\t', angle2) print('hip joint:\t', hip_angle*180/np.pi) print('knee joint:\t', knee_angle*180/np.pi) print('linear position:\t', slider_pos) print('-'*50) cnt += 0.1
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
from controller import Robot import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 150*np.pi/180 + q_offset s_max = np.sqrt((R+L)**2-e**2) robot = Robot() TIME_STEP = int(robot.getBasicTimeStep()) motors = [] motor_names = ['hip motor', 'knee linear motor'] position_sensors = [] position_sensor_names = ['hip position sensor', 'knee position sensor', 'slider position sensor'] q_pre = np.array([0, 0]) def get_data(): global q_pre q = [0, 0] qd = [0, 0] q[0] = position_sensors[0].getValue() q[1] = position_sensors[1].getValue() # q[2] = position_sensors[2].getValue() qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP # qd[2] = q[2] - q_pre[2] q_pre = q + np.array([0, 0]) return np.array(q), np.array(qd) # 逆运动学(二维) def inverse_kinematic(x, y): theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2)) theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y)) print('des knee angle:\t', theta2 * 180 / np.pi) print('des hip angle:\t', theta1 * 180 / np.pi) alpha = Q + theta2 # print('alpha:\t', alpha * 180 / np.pi) s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L return [theta1, s] def forward_kinematic(angle1, angle2): x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2) z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2) return np.array([x, z]) def jacobian(theta1, theta2): J = np.zeros([2, 2]) J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2) J[0, 1] = -L2 * np.cos(theta1 + theta2) J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) J[1, 1] = L2 * np.sin(theta1 + theta2) return J def dynamic(force): theta1 = position_sensors[0].getValue() theta2 = position_sensors[1].getValue() tau = np.matmul(jacobian(theta1, theta2).transpose(), force) print('tau: ', tau) alpha = Q + theta2 c_alpha = np.cos(alpha) s_alpha = np.sin(alpha) s_beta = (R * s_alpha - e) / L c_beta = 1 - 0.5 * s_beta ** 2 m1 = R * (s_alpha*c_beta + c_alpha*s_beta) P = tau[1] * c_beta / m1 return [tau[0], P] def stand(height): kp = np.array([[10, 0], [0, 1500]]) kd = np.array([[1, 0], [0, 500]]) p_des = np.array([0, -height]) p_dot_des = np.array([0, 0]) q, qd = get_data() # print(q) # print(qd) p_current = forward_kinematic(q[0], q[1]) p_dot_current = np.matmul(jacobian(q[0], q[1]), qd) print('p_dot_current:\t', p_dot_current) print('p_current:\t', p_current) force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current) print('force:\t', force) tau = dynamic(force) print('command:\t', tau) return tau def init(): for i in range(len(motor_names)): motors.append(robot.getDevice(motor_names[i])) for i in range(len(position_sensor_names)): position_sensors.append((robot.getDevice(position_sensor_names[i]))) position_sensors[i].enable(TIME_STEP) # angle1, angle2 = inverse_kinematic(0, -0.5) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) cnt = 0 init() while robot.step(TIME_STEP) != -1: # angle1, angle2 = inverse_kinematic(0, height) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) # height = 0.075*np.sin(cnt) + 0.425 tau = stand(0.2) # tau = [0, 100] motors[0].setTorque(tau[0]) motors[1].setForce(tau[1]) hip_angle = position_sensors[0].getValue() knee_angle = position_sensors[1].getValue() slider_pos = position_sensors[2].getValue() print('L_bar:\t', s_max) # print('S:\t', angle2) print('hip joint:\t', hip_angle*180/np.pi) print('knee joint:\t', knee_angle*180/np.pi) print('linear position:\t', slider_pos) print('-'*50) cnt += 0.1
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
复制
from controller import Robot import numpy as np import matplotlib.pyplot as plt R = 0.05 L = 0.1 L1 = 0.2 L2 = 0.2 e = 0.03 q_offset = np.arcsin(e/L1) Q = 150*np.pi/180 + q_offset s_max = np.sqrt((R+L)**2-e**2) robot = Robot() TIME_STEP = int(robot.getBasicTimeStep()) motors = [] motor_names = ['hip motor', 'knee linear motor'] position_sensors = [] position_sensor_names = ['hip position sensor', 'knee position sensor', 'slider position sensor'] q_pre = np.array([0, 0]) def get_data(): global q_pre q = [0, 0] qd = [0, 0] q[0] = position_sensors[0].getValue() q[1] = position_sensors[1].getValue() # q[2] = position_sensors[2].getValue() qd[0] = (q[0] - q_pre[0])*1000/TIME_STEP qd[1] = (q[1] - q_pre[1])*1000/TIME_STEP # qd[2] = q[2] - q_pre[2] q_pre = q + np.array([0, 0]) return np.array(q), np.array(qd) # 逆运动学(二维) def inverse_kinematic(x, y): theta2 = -np.pi + np.arccos((L1**2 + L2**2-x**2-y**2)/(2*L1*L2)) theta1 = np.arccos((x**2+y**2+L1**2-L2**2)/(2*np.sqrt(x**2+y**2)*L1))-np.arctan2(x, abs(y)) print('des knee angle:\t', theta2 * 180 / np.pi) print('des hip angle:\t', theta1 * 180 / np.pi) alpha = Q + theta2 # print('alpha:\t', alpha * 180 / np.pi) s = s_max - L - R * np.cos(alpha) + 0.5 * (R * np.sin(alpha) - e) ** 2 / L return [theta1, s] def forward_kinematic(angle1, angle2): x = -L1 * np.sin(angle1) - L2 * np.sin(angle1+angle2) z = -L1 * np.cos(angle1) - L2 * np.cos(angle1+angle2) return np.array([x, z]) def jacobian(theta1, theta2): J = np.zeros([2, 2]) J[0, 0] = -L1 * np.cos(theta1) - L2 * np.cos(theta1 + theta2) J[0, 1] = -L2 * np.cos(theta1 + theta2) J[1, 0] = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2) J[1, 1] = L2 * np.sin(theta1 + theta2) return J def dynamic(force): theta1 = position_sensors[0].getValue() theta2 = position_sensors[1].getValue() tau = np.matmul(jacobian(theta1, theta2).transpose(), force) print('tau: ', tau) alpha = Q + theta2 c_alpha = np.cos(alpha) s_alpha = np.sin(alpha) s_beta = (R * s_alpha - e) / L c_beta = 1 - 0.5 * s_beta ** 2 m1 = R * (s_alpha*c_beta + c_alpha*s_beta) P = tau[1] * c_beta / m1 return [tau[0], P] def stand(height): kp = np.array([[10, 0], [0, 1500]]) kd = np.array([[1, 0], [0, 500]]) p_des = np.array([0, -height]) p_dot_des = np.array([0, 0]) q, qd = get_data() # print(q) # print(qd) p_current = forward_kinematic(q[0], q[1]) p_dot_current = np.matmul(jacobian(q[0], q[1]), qd) print('p_dot_current:\t', p_dot_current) print('p_current:\t', p_current) force = np.matmul(kp, p_des-p_current) + np.matmul(kd, p_dot_des-p_dot_current) print('force:\t', force) tau = dynamic(force) print('command:\t', tau) return tau def init(): for i in range(len(motor_names)): motors.append(robot.getDevice(motor_names[i])) for i in range(len(position_sensor_names)): position_sensors.append((robot.getDevice(position_sensor_names[i]))) position_sensors[i].enable(TIME_STEP) # angle1, angle2 = inverse_kinematic(0, -0.5) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) cnt = 0 init() while robot.step(TIME_STEP) != -1: # angle1, angle2 = inverse_kinematic(0, height) # motors[0].setPosition(angle1) # motors[1].setPosition(angle2) # height = 0.075*np.sin(cnt) + 0.425 tau = stand(0.2) # tau = [0, 100] motors[0].setTorque(tau[0]) motors[1].setForce(tau[1]) hip_angle = position_sensors[0].getValue() knee_angle = position_sensors[1].getValue() slider_pos = position_sensors[2].getValue() print('L_bar:\t', s_max) # print('S:\t', angle2) print('hip joint:\t', hip_angle*180/np.pi) print('knee joint:\t', knee_angle*180/np.pi) print('linear position:\t', slider_pos) print('-'*50) cnt += 0.1
评论(0)
您还未登录,请登录后发表或查看评论