Example #1
0
    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
Example #2
0
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.