def __init__(self, goal, zmp_args, feet, step):
Ctrl.__init__(self)
self._goal = goal
self.feet = feet
self.step = step
self.zmp_ctrl = ZMPCtrl([], **zmp_args)
Kp, Kd, weight = feet['Kp'], feet['Kd'], feet['weight']
self.l_foot_ctrl = KpTrajCtrl([None, None, None], Kp, Kd)
self.r_foot_ctrl = KpTrajCtrl([None, None, None], Kp, Kd)
self.l_foot_task = FrameTask(feet['l_frame'], self.l_foot_ctrl, \
[], weight, 0, False, "l_foot")
self.r_foot_task = FrameTask(feet['r_frame'], self.r_foot_ctrl, \
[], weight, 0, False, "r_foot")
self.cdof = zmp_args['cdof']
self.dt = zmp_args['dt']
self._R0 = feet["R0"][0:3, 0:3]
self._iR0 = self._R0.T
self.events = []
self._num_step = 0
self._sequence = []
self._prev_foot = None
self._next_foot = None
self._foot_traj = None
class WalkingCtrl(Ctrl):
def __init__(self, goal, zmp_args, feet, step):
Ctrl.__init__(self)
self._goal = goal
self.feet = feet
self.step = step
self.zmp_ctrl = ZMPCtrl([], **zmp_args)
Kp, Kd, weight = feet['Kp'], feet['Kd'], feet['weight']
self.l_foot_ctrl = KpTrajCtrl([None, None, None], Kp, Kd)
self.r_foot_ctrl = KpTrajCtrl([None, None, None], Kp, Kd)
self.l_foot_task = FrameTask(feet['l_frame'], self.l_foot_ctrl, \
[], weight, 0, False, "l_foot")
self.r_foot_task = FrameTask(feet['r_frame'], self.r_foot_ctrl, \
[], weight, 0, False, "r_foot")
self.cdof = zmp_args['cdof']
self.dt = zmp_args['dt']
self._R0 = feet["R0"][0:3, 0:3]
self._iR0 = self._R0.T
self.events = []
self._num_step = 0
self._sequence = []
self._prev_foot = None
self._next_foot = None
self._foot_traj = None
def init(self, world, LQP_ctrl):
self.world = world
self.LQP_ctrl = LQP_ctrl
self.set_goal(self._goal)
def set_goal(self, new_goal):
assert(isinstance(new_goal, dict))
action = new_goal["action"]
if action == 'idle':
mid_feet = self._get_center_of_feet()
self.zmp_ctrl.set_goal([mid_feet])
elif action == 'goto':
start = self._get_center_of_feet()
end = asarray(new_goal["pos"])
vect = (end - start)
if "angle" in new_goal:
angle = new_goal["angle"]
else:
angle = arctan2(vect[1], vect[0])
traj = array([linspace(start[0], end[0], 10000), \
linspace(start[1], end[1], 10000), \
angle*ones(10000)]).T
s = self.step
l_start, r_start = self._get_lf_pose(), self._get_rf_pose()
points = traj2zmppoints(traj, s["length"], s["side"], \
l_start, r_start, s["start"])
zmp_ref = zmppoints2zmptraj(points, s["time"], self.dt)
self.zmp_ctrl.set_goal(zmp_ref)
ftraj = zmppoints2foottraj(points, s["time"], \
s["ratio"], s["height"], \
self.dt, self.cdof, self._R0)
self._sequence = self.world.current_time + \
(arange(len(ftraj)+1) + .5)*self.step["time"]
self._foot_traj = ftraj
self._num_step = 0
self._next_foot = self.step['start']
self._prev_foot = 'right' if \
self.step['start'] == 'left' else 'left'
elif action == 'one step':
if "angle" in new_goal:
angle = new_goal["angle"]
else:
alf = self._get_lf_pose()[2]
arf = self._get_rf_pose()[2]
angle = (alf+arf)/2.
fpose = (self._get_lf_pose() + self._get_rf_pose() )/2.
forward = array( [ cos(fpose[2]), sin(fpose[2]) ] )
left_dir = array( [-sin(fpose[2]), cos(fpose[2]) ] )
l_start = self._get_lf_pose()
r_start = self._get_rf_pose()
s = self.step
if s["start"] == 'left':
lpose = s["length"]*forward + s["side"]*left_dir
points = [l_start, r_start, (lpose[0], lpose[1], angle)]
elif s["start"] == 'right':
rpose = s["length"]*forward - s["side"]*left_dir # minus because if right foot
points = [r_start, l_start, (rpose[0], rpose[1], angle)]
else:
raise ValueError
zmp_ref = zmppoints2zmptraj(points, s["time"], self.dt)
self.zmp_ctrl.set_goal(zmp_ref)
ftraj = zmppoints2foottraj(points, s["time"], \
s["ratio"], s["height"], \
self.dt, self.cdof, self._R0)
self._sequence = self.world.current_time + \
(arange(len(ftraj)+1) + .5)*self.step["time"]
self._foot_traj = ftraj
self._num_step = 0
self._next_foot = self.step['start']
self._prev_foot = 'right' if \
self.step['start'] == 'left' else 'left'
def update(self, rstate, dt):
if len(self._sequence) and self._num_step < len(self._sequence):
t = self.world.current_time
sqt = self._sequence[self._num_step]
r = self.step['time']*(1 - self.step['ratio'])/2.
if t >= sqt - r:
#print "desactivate FOOT", self._prev_foot
self._end_foot(self._prev_foot)
if t >= sqt + r:
print "ACTIVATE FOOT", self._next_foot
if self._num_step < len(self._foot_traj):
self._start_foot(self._next_foot, \
self._foot_traj[self._num_step])
self._prepare_to_next_foot()
def _end_foot(self, foot):
if foot == 'left' :
task = self.l_foot_task
const = self.feet['l_const']
elif foot == 'right':
task = self.r_foot_task
const = self.feet['r_const']
for c in const:
self.LQP_ctrl.is_enabled[c] = True
task._is_active = False
def _start_foot(self, foot, traj):
if foot == 'left' :
task = self.l_foot_task
const = self.feet['l_const']
elif foot == 'right':
task = self.r_foot_task
const = self.feet['r_const']
for c in const:
self.LQP_ctrl.is_enabled[c] = False
task._is_active = True
task.ctrl.set_goal(traj)
def _prepare_to_next_foot(self):
tmp = self._next_foot
self._next_foot = self._prev_foot
self._prev_foot = tmp
self._num_step += 1
def _get_foot_pose(self, pose):
pos = pose[self.cdof, 3]
H0a0 = dot(pose[0:3, 0:3], self._iR0)
angle = arctan2(H0a0[self.cdof[1], self.cdof[0]], \
H0a0[self.cdof[0], self.cdof[0]])
return array([pos[0], pos[1], angle])
def _get_lf_pose(self):
return self._get_foot_pose(self.feet["l_frame"].pose)
def _get_rf_pose(self):
return self._get_foot_pose(self.feet["r_frame"].pose)
def _get_center_of_feet(self):
lf = self._get_lf_pose()[0:2]
rf = self._get_rf_pose()[0:2]
return (lf+rf)/2.