You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
311 lines
13 KiB
311 lines
13 KiB
import numpy as np
|
|
from zmp_controller.TrajectoryGenerator import create_multiple_trajectory
|
|
import time
|
|
from zmp_controller.ikine import IKineQuadruped
|
|
|
|
|
|
class Action4():
|
|
def __init__(self,
|
|
freq=300,
|
|
kinematic_scheme='x',
|
|
ef_init_x=0.149,
|
|
ef_init_y=0.13,
|
|
cog_offset_x=0.0,
|
|
cog_offset_y=0.0,
|
|
robot_height=0.17,
|
|
kp=[12, 12, 12],
|
|
kd=[0.1, 0.1, 0.1]):
|
|
|
|
self.freq = freq
|
|
self.kinematic_scheme = kinematic_scheme
|
|
self.ef_init_x = ef_init_x
|
|
self.ef_init_y = ef_init_y
|
|
self.cog_offset_x = cog_offset_x
|
|
self.cog_offset_y = cog_offset_y
|
|
self.robot_height = robot_height
|
|
self.max_kp = np.array(kp[:3])
|
|
self.max_kd = np.array(kd[:3]) + [0.4, 0.4, 0.4]
|
|
|
|
self.finished = False
|
|
self.cur_joint_pos = np.array([0.0]*12)
|
|
self.ref_joint_pos = np.array([0.0]*12)
|
|
self.ref_joint_vel = np.array([0.0]*12)
|
|
self.ref_joint_torq = np.array([0.0]*12)
|
|
self.ref_joint_kp = np.array([0.0]*12)
|
|
self.ref_joint_kd = np.array([0.0]*12)
|
|
|
|
self.ik = IKineQuadruped(theta_offset=[0, -np.pi/2, 0])
|
|
|
|
self.start_time = int(time.time()*1000.0)
|
|
|
|
self.all_theta_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
self.all_vel_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
self.all_torq_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
self.all_kp_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
self.all_kd_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
|
|
# print(self.max_kp)
|
|
|
|
def is_finished(self):
|
|
return self.finished
|
|
|
|
def set_cur_joint_pos(self, cur_pos):
|
|
self.cur_joint_pos = cur_pos[:]
|
|
|
|
def get_ref_joint_pos(self):
|
|
return self.ref_joint_pos, self.ref_joint_vel, self.ref_joint_torq, self.ref_joint_kp, self.ref_joint_kd
|
|
|
|
def reset(self):
|
|
self.all_theta_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
self.all_vel_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
self.all_torq_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
self.all_kp_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
self.all_kd_refs = [[], [], [], [], [], [], [], [], [], [], [], []]
|
|
|
|
def step(self, it):
|
|
self.ref_joint_pos = []
|
|
self.ref_joint_vel = [0]*12
|
|
self.ref_joint_torq = [0]*12
|
|
self.ref_joint_kp = []
|
|
self.ref_joint_kd = []
|
|
|
|
if it >= len(self.all_theta_refs[0]):
|
|
finished = True
|
|
for i in range(12):
|
|
self.ref_joint_pos.append(self.all_theta_refs[i][-1])
|
|
self.ref_joint_kp.append(self.all_kp_refs[i][-1])
|
|
self.ref_joint_kd.append(self.all_kd_refs[i][-1])
|
|
else:
|
|
finished = False
|
|
for i in range(12):
|
|
self.ref_joint_pos.append(self.all_theta_refs[i][it])
|
|
self.ref_joint_kp.append(self.all_kp_refs[i][it])
|
|
self.ref_joint_kd.append(self.all_kd_refs[i][it])
|
|
|
|
# print(self.ref_joint_kp[0])
|
|
|
|
return finished, self.ref_joint_pos, self.ref_joint_vel, self.ref_joint_torq, self.ref_joint_kp, self.ref_joint_kd
|
|
|
|
def execute(self):
|
|
self.finished = False
|
|
|
|
# Put your code here
|
|
|
|
self.ref_joint_kp = [self.max_kp[0], self.max_kp[1], self.max_kp[2]]*4
|
|
self.ref_joint_kd = [self.max_kd[0], self.max_kd[1], self.max_kd[2]]*4
|
|
|
|
|
|
# self.ref_joint_pos = np.array(self.cur_joint_pos[:])
|
|
# time.sleep(0.1)
|
|
theta_cur = list(self.cur_joint_pos[:])
|
|
for i in range(12):
|
|
self.all_theta_refs[i].append(theta_cur[i])
|
|
self.all_kp_refs[i].append(self.ref_joint_kp[i])
|
|
self.all_kd_refs[i].append(self.ref_joint_kd[i])
|
|
|
|
# lay down
|
|
offs_z = 0.1
|
|
# theta_ref = theta_cur[:]
|
|
theta_ref = self.ik.calculate([ self.ef_init_x+self.cog_offset_x, -self.ef_init_y, -self.robot_height+offs_z,
|
|
self.ef_init_x+self.cog_offset_x, self.ef_init_y, -self.robot_height+offs_z,
|
|
-self.ef_init_x+self.cog_offset_x, -self.ef_init_y, -self.robot_height+offs_z,
|
|
-self.ef_init_x+self.cog_offset_x, self.ef_init_y, -self.robot_height+offs_z], config=self.kinematic_scheme)
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.4, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
|
|
# print("1")
|
|
# time.sleep(0.2)
|
|
|
|
# go to back
|
|
theta_cur = theta_ref[:]
|
|
theta_ref = self.ik.calculate([ self.ef_init_x+self.cog_offset_x, -self.ef_init_y+0.04, -self.robot_height-0.07,
|
|
self.ef_init_x+self.cog_offset_x, self.ef_init_y-0.04, -self.robot_height+offs_z,
|
|
-self.ef_init_x+self.cog_offset_x, -self.ef_init_y+0.04, -self.robot_height-0.07,
|
|
-self.ef_init_x+self.cog_offset_x, self.ef_init_y-0.04, -self.robot_height+offs_z], config=self.kinematic_scheme)
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.4, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# print("2")
|
|
# print(theta_ref)
|
|
# time.sleep(10.2)
|
|
|
|
theta_cur = theta_ref[:]
|
|
theta_ref = self.ik.calculate([ self.ef_init_x+self.cog_offset_x, -self.ef_init_y+0.04, -self.robot_height-0.07,
|
|
self.ef_init_x+self.cog_offset_x, self.ef_init_y+0.04, -self.robot_height,
|
|
-self.ef_init_x+self.cog_offset_x, -self.ef_init_y+0.04, -self.robot_height-0.07,
|
|
-self.ef_init_x+self.cog_offset_x, self.ef_init_y+0.04, -self.robot_height], config=self.kinematic_scheme)
|
|
theta_ref[0] = -0.2
|
|
theta_ref[1] = -2.0
|
|
theta_ref[2] = 4.6
|
|
theta_ref[6] = 0.2
|
|
theta_ref[7] = -2.0
|
|
theta_ref[8] = 4.6
|
|
# print("set kp")
|
|
# # self.ref_joint_kp[1] = 2
|
|
# self.ref_joint_kp[2] = 2
|
|
# # self.ref_joint_kp[7] = 2
|
|
# self.ref_joint_kp[8] = 2
|
|
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.4, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# print("3")
|
|
# print(theta_ref)
|
|
# time.sleep(10.2)
|
|
|
|
theta_cur = theta_ref[:]
|
|
theta_ref = [0, -1.57, 3.14,
|
|
0, 1.57, -3.14,
|
|
0, -1.57, 3.14,
|
|
0, 1.57, -3.14]
|
|
# self.ref_joint_kp[2] = 10
|
|
# self.ref_joint_kp[8] = 10
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.4, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# print("4")
|
|
# time.sleep(0.2)
|
|
|
|
theta_cur = theta_ref[:]
|
|
theta_ref = [-0.8, -1.57, 3.14,
|
|
0.5, 1.9, -4.5,
|
|
0.8, -1.57, 3.14,
|
|
-0.5, 1.9, -4.5]
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.2, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# print("5")
|
|
# time.sleep(3.2)
|
|
|
|
theta_cur = theta_ref[:]
|
|
theta_ref = [-1.3, -1.57, 3.14,
|
|
0.36, 2.96, -5.81,
|
|
1.3, -1.57, 3.14,
|
|
-0.36, 2.96, -5.81]
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.25, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# print("6")
|
|
# time.sleep(5.5)
|
|
|
|
theta_cur = theta_ref[:]
|
|
theta_ref = [-1.1, -1.57, 3.14,
|
|
-0.2, 1.3, -1.88,
|
|
1.1, -1.57, 3.14,
|
|
0.2, 1.3, -1.88]
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.2, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# print("7")
|
|
time.sleep(0.2)
|
|
|
|
theta_cur = theta_ref[:]
|
|
theta_ref = [0, -1.57, 3.14, 0, 1.57, -3.14, 0, -1.57, 3.14, 0, 1.57, -3.14]
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.4, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# time.sleep(2)
|
|
l = 0.09585
|
|
abad_attach_y = 0.066
|
|
angle = np.arccos((self.ef_init_y-abad_attach_y)/l)
|
|
|
|
theta_cur = theta_ref[:]
|
|
# theta_ref = np.array(self.cur_joint_pos[:])
|
|
theta_ref[0] = angle
|
|
theta_ref[6] = -angle
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.3, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# time.sleep(1)
|
|
|
|
theta_cur = theta_ref[:]
|
|
# theta_ref = np.array(self.cur_joint_pos[:])
|
|
theta_ref[3] = -angle
|
|
theta_ref[9] = angle
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.3, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
theta_cur = theta_ref[:]
|
|
theta_ref = self.ik.calculate([ self.ef_init_x+self.cog_offset_x, -self.ef_init_y, -self.robot_height,
|
|
self.ef_init_x+self.cog_offset_x, self.ef_init_y, -self.robot_height,
|
|
-self.ef_init_x+self.cog_offset_x, -self.ef_init_y, -self.robot_height,
|
|
-self.ef_init_x+self.cog_offset_x, self.ef_init_y, -self.robot_height], config=self.kinematic_scheme)
|
|
theta_refs = create_multiple_trajectory(theta_cur, theta_ref, 0.6, 1/self.freq)
|
|
# self.take_position(theta_refs)
|
|
for i in range(12):
|
|
self.all_theta_refs[i] += theta_refs[i]
|
|
self.all_kp_refs[i] += [self.ref_joint_kp[i]]*len(theta_refs[i])
|
|
self.all_kd_refs[i] += [self.ref_joint_kd[i]]*len(theta_refs[i])
|
|
|
|
# Finish
|
|
|
|
# self.finished = True
|
|
|
|
# Put your functions below
|
|
|
|
def kpkd_inc(self, t):
|
|
ts = self.freq*t
|
|
kp_inc = self.max_kp[:3]/ts
|
|
kd_inc = self.max_kd[:3]/ts
|
|
|
|
for i in range(3):
|
|
if self.ref_joint_kp[i] < self.max_kp[i]:
|
|
self.ref_joint_kp[i] += kp_inc[i]
|
|
self.ref_joint_kp[i+3] += kp_inc[i]
|
|
self.ref_joint_kp[i+6] += kp_inc[i]
|
|
self.ref_joint_kp[i+9] += kp_inc[i]
|
|
|
|
if self.ref_joint_kd[i] < self.max_kd[i]:
|
|
self.ref_joint_kd[i] += kd_inc[i]
|
|
self.ref_joint_kd[i+3] += kd_inc[i]
|
|
self.ref_joint_kd[i+6] += kd_inc[i]
|
|
self.ref_joint_kd[i+9] += kd_inc[i]
|
|
|
|
# time.sleep(1/self.freq)
|
|
|
|
def kpkd_null(self):
|
|
self.ref_joint_kp = np.array([0.0]*12)
|
|
self.ref_joint_kd = np.array([0.0]*12)
|
|
# time.sleep(10/self.freq)
|