Beispiel #1
0
class Fivel_Controller(object):
    """Fivel_Controller"""
    def __init__(self, arg):
        super(Fivel_Controller, self).__init__()

        ##################################################################
        ######################## GAIT PARAMETERS #########################
        ##################################################################
        if len(arg) < 10:
            self.SwingEff0 = -1.2  # Default swing effort
            self.SupAnkSetPoint = -0.04  # Setpoint for support ankle
            self.SupAnkGain_p = 30  # Support ankle gain p
            self.SupAnkGain_d = 10  # Support ankle gain d
            self.SwingEffDur = 0.50  # Duration of swing effort
            self.FootExtEff = 1.0  # Foot extension effort
            self.FootExtDur = 0.22  # Foot extension duration
            self.ToeOffEff = 17.65  # Toe-off effort
            self.ToeOffDur = 0.24  # Toe-off duration
            self.DesTorsoAng = -0.055  # Desired torso angle
        else:
            self.SwingEff0 = arg[0]  # Default swing effort
            self.SupAnkSetPoint = arg[1]  # Setpoint for support ankle
            self.SupAnkGain_p = arg[2]  # Support ankle gain p
            self.SupAnkGain_d = arg[3]  # Support ankle gain d
            self.SwingEffDur = arg[4]  # Duration of swing effort
            self.FootExtEff = arg[5]  # Foot extension effort
            self.FootExtDur = arg[6]  # Foot extension duration
            self.ToeOffEff = arg[7]  # Toe-off effort
            self.ToeOffDur = arg[8]  # Toe-off duration
            self.DesTorsoAng = arg[9]  # Desired torso angle

        self.StepsTaken = 0
        self.ApertureMean = 0
        self.ApertureStd = 0
        self.GlobalPos = 0

        self.reset_srv = rospy.ServiceProxy('/gazebo/reset_models', Empty)

        ##################################################################
        ###################### RAISE FOOT SEQUENCE #######################
        ##################################################################

        self.pos1 = zeros(4)

        # "left_hip","left_ankle","right_hip","right_ankle"
        #      0           1           2            3

        # Sequence 1 for raising the left foot (from home position)
        self.seq1_1 = copy(self.pos1)
        self.seq1_1[3] = -0.05
        self.seq1_1[0] = 0.25
        self.seq1_1[1] = -0.25

        self.seq1_2 = copy(self.seq1_1)
        self.seq1_2[1] = -1.49

        # Sequence 2 for raising the right foot (from home position)
        self.seq2_1 = copy(self.pos1)
        self.seq2_1[0] = 0.04
        self.seq2_1[1] = -0.04
        self.seq2_1[2] = 0.25
        self.seq2_1[3] = -0.25

        self.seq2_2 = copy(self.seq2_1)
        self.seq2_2[3] = -1.49

        ##################################################################
        ########################## INITIALIZE ############################
        ##################################################################

        self.RobotName = "ghost_fivel"
        self.JointNames = [
            "left_hip", "left_ankle", "right_hip", "right_ankle"
        ]

        # Initialize joint commands handler
        self.JC = JointCommands_msg_handler(self.RobotName, self.JointNames)

        # Initialize robot state listener
        self.RS = robot_state(self.JointNames)
        self.MsgSub = rospy.Subscriber('/' + self.RobotName + '/robot_state',
                                       RobotState, self.RS_cb)
        self.OdomSub = rospy.Subscriber('/ground_truth_odom', Odometry,
                                        self.Odom_cb)

    ##################################################################
    ########################### FUNCTIONS ############################
    ##################################################################

    # def RaiseLeg(self,which):
    #     if which == "inner":
    #         self.JC.send_pos_traj(self.pos1,self.seq1_1,1,0.01)
    #         rospy.sleep(0.2)
    #         self.JC.send_pos_traj(self.seq1_1,self.seq1_2,1,0.01)
    #         self.JC.set_eff('inner_ankle',-5)
    #         self.JC.send_pos_traj(self.seq1_2,self.seq1_3,1,0.01)
    #         rospy.sleep(0.2)
    #     if which == "outer":
    #         self.JC.send_pos_traj(self.pos1,self.seq2_1,2,0.01)
    #         rospy.sleep(0.2)
    #         self.JC.send_pos_traj(self.seq2_1,self.seq2_2,2,0.01)
    #         self.JC.set_eff('left_ankle',-5)
    #         self.JC.set_eff('right_ankle',-5)
    #         self.JC.send_command()
    #         #self.JC.send_pos_traj(self.seq2_2,self.seq2_3,1,0.01)
    #         rospy.sleep(0.2)

    def StandOn(self, leg, setpoint, time, dt):
        StartTime = rospy.Time.now().to_sec()
        while rospy.Time.now().to_sec() - StartTime < time:
            # Apply closed loop torque for upright torso
            hip_eff = self.BalanceCmd(leg, setpoint)

            self.JC.set_eff(leg + '_hip', hip_eff)
            self.JC.send_command()
            rospy.sleep(dt)

    def BalanceCmd(self, which, setpoint):
        ank_pos = self.RS.GetJointPos(which + "_ankle")
        hip_pos = self.RS.GetJointPos(which + "_hip")
        ank_vel = self.RS.GetJointVel(which + "_ankle")
        hip_vel = self.RS.GetJointVel(which + "_hip")

        torso_angle = ank_pos + hip_pos
        torso_angvel = ank_vel + hip_vel

        # Apply closed loop torque for upright torso
        hip_eff = 200 * (setpoint - torso_angle) + 50 * (0 - torso_angvel)
        return hip_eff

    def TakeStep(self, which):
        #Aperture = abs(self.RS.GetJointPos('left_hip')-self.RS.GetJointPos('right_hip'))
        SwingEff = self.SwingEff0  #+ self.kAp * (self.DesAp - Aperture)
        DesTorsoAng = self.DesTorsoAng
        # if which == "left":
        #     DesTorsoAng = self.DesTorsoAng - 0.1*self.RS.GetJointPos('right_hip')
        # if which == "right":
        #     DesTorsoAng = self.DesTorsoAng - 0.1*self.RS.GetJointPos('left_hip')

        if which == "left":
            # Swing leg 0.4 sec and incline stance leg
            self.JC.set_eff("left_hip", SwingEff)
            self.JC.send_command()
            self.StandOn("right", DesTorsoAng, self.SwingEffDur, 0.005)
            # Straighten ankle and free swing
            self.JC.set_eff("left_ankle", self.FootExtEff)
            self.JC.set_eff("left_hip", 0)
            self.JC.send_command()
            self.StandOn("right", DesTorsoAng, self.FootExtDur, 0.005)
            # Toe off
            self.JC.set_eff("left_ankle", 0)
            self.JC.set_pos("left_ankle", self.SupAnkSetPoint)
            self.JC.set_gains("left_ankle", self.SupAnkGain_p, 0,
                              self.SupAnkGain_d)
            self.JC.set_eff("right_ankle", self.ToeOffEff)
            self.JC.send_command()
            self.StandOn("left", DesTorsoAng, self.ToeOffDur, 0.005)
            # Bend foot for clearance
            self.JC.set_eff("right_ankle", -2)
            self.JC.set_eff("right_hip", 0)
            self.JC.send_command()

        if which == "right":
            # Swing leg 0.4 sec and incline stance leg
            self.JC.set_eff("right_hip", SwingEff)
            self.JC.send_command()
            self.StandOn("left", DesTorsoAng, self.SwingEffDur, 0.005)
            # Straighten ankle and free swing
            self.JC.set_eff("right_ankle", self.FootExtEff)
            self.JC.set_eff("right_hip", 0)
            self.JC.send_command()
            self.StandOn("left", DesTorsoAng, self.FootExtDur, 0.005)
            # Toe off
            self.JC.set_eff("right_ankle", 0)
            self.JC.set_pos("right_ankle", self.SupAnkSetPoint)
            self.JC.set_gains("right_ankle", self.SupAnkGain_p, 0,
                              self.SupAnkGain_d)
            self.JC.set_eff("left_ankle", self.ToeOffEff)
            self.JC.send_command()
            self.StandOn("right", DesTorsoAng, self.ToeOffDur, 0.005)
            # Bend foot for clearance
            self.JC.set_eff("left_ankle", -2)
            self.JC.set_eff("left_hip", 0)
            self.JC.send_command()

    # def TakeStep(self,which):
    #     TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

    #     if TimeElapsed<60:
    #         Aperture = abs(self.RS.GetJointPos('left_hip'))
    #         self.SwingEff = self.SwingEff0 + self.kAp*(self.DesAp - Aperture)
    #         #self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)

    #         if which == 'outer':
    # Aperture = self.RS.GetJointPos('left_hip')
    #         else:
    # Aperture = -self.RS.GetJointPos('left_hip')
    #         self.ToeOffEff = self.ToeOffEff0 - 1*Aperture
    #         #self.SupAnkSetPoint = -0.02 + 0.03 * Aperture

    #         print Aperture
    #     else:
    #         Aperture = abs(self.RS.GetJointPos('left_hip'))
    #         #self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)
    #         #print self.ToeOffEff
    #         #print self.SwingEff
    #     if which == "outer":
    #         # Toe off
    #         self.JC.set_gains('inner_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
    #         self.JC.set_pos('inner_ankle',self.SupAnkSetPoint)
    #         self.JC.set_eff('left_ankle',self.ToeOffEff)
    #         self.JC.set_eff('right_ankle',self.ToeOffEff)
    #         self.JC.set_eff('left_hip',0)
    #         self.JC.set_eff('right_hip',0)
    #         self.JC.send_command()
    #         rospy.sleep(self.ToeOffDur)
    #         # Raise feet and swing
    #         self.JC.set_eff('left_ankle',-3)
    #         self.JC.set_eff('right_ankle',-3)
    #         self.JC.set_eff('left_hip',-self.SwingEff)
    #         self.JC.set_eff('right_hip',-self.SwingEff)
    #         self.JC.send_command()
    #         rospy.sleep(self.TotSwingDur-self.FootExtDur-self.FreeSwingDur)
    #         # Lower feet
    #         self.JC.set_eff('left_ankle',self.FootExtEff)
    #         self.JC.set_eff('right_ankle',self.FootExtEff)
    #         self.JC.send_command()
    #         rospy.sleep(self.FootExtDur)
    #         # End swing
    #         self.JC.set_eff('left_hip',0)
    #         self.JC.set_eff('right_hip',0)
    #         self.JC.set_eff('left_ankle',0)
    #         self.JC.set_eff('right_ankle',0)
    #         self.JC.send_command()
    #         rospy.sleep(self.FreeSwingDur)

    #     if which == "inner":
    #         # Toe off
    #         self.JC.set_gains('left_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
    #         self.JC.set_gains('right_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
    #         self.JC.set_pos('left_ankle',self.SupAnkSetPoint)
    #         self.JC.set_pos('right_ankle',self.SupAnkSetPoint)
    #         self.JC.set_eff('inner_ankle',self.ToeOffEff)
    #         self.JC.set_eff('left_hip',0)
    #         self.JC.set_eff('right_hip',0)
    #         self.JC.send_command()
    #         rospy.sleep(self.ToeOffDur)
    #         # Raise feet and swing
    #         self.JC.set_eff('inner_ankle',-3)
    #         self.JC.set_eff('left_hip',self.SwingEff)
    #         self.JC.set_eff('right_hip',self.SwingEff)
    #         self.JC.send_command()
    #         rospy.sleep(self.TotSwingDur-self.FootExtDur-self.FreeSwingDur)
    #         # Lower feet
    #         self.JC.set_eff('inner_ankle',self.FootExtEff)
    #         self.JC.send_command()
    #         rospy.sleep(self.FootExtDur)
    #         # End swing
    #         self.JC.set_eff('left_hip',0)
    #         self.JC.set_eff('right_hip',0)
    #         self.JC.set_eff('inner_ankle',0)
    #         self.JC.send_command()
    #         rospy.sleep(self.FreeSwingDur)

    def ResetPose(self):
        self.JC.set_pos('left_hip', 0)
        self.JC.set_pos('left_ankle', 0)
        self.JC.set_pos('right_hip', 0)
        self.JC.set_pos('right_ankle', 0)
        self.JC.send_command()

    def RS_cb(self, msg):
        self.RS.UpdateState(msg)

    def Odom_cb(self, msg):
        self.GlobalPos = msg.pose.pose.position

    def reset(self):
        self.reset_srv()
        rospy.sleep(1.5)

        while self.GlobalPos.z < 0.97 or self.GlobalPos.z > 1.03 or abs(
                self.GlobalPos.x) > 0.5:
            self.reset_srv()
            rospy.sleep(1)

    def Run(self, TimeOut=0):
        rospy.sleep(0.1)

        # Start with left leg raised
        # self.JC.send_pos_traj(self.RS.GetJointPos(),self.seq1_2,0.5,0.01)
        # rospy.sleep(0.5)
        # self.reset()
        # self.StandOn("right",10,0.005)

        # Start with right leg raised
        self.JC.set_gains('right_hip', 2000, 0, 150)
        self.JC.set_gains('left_hip', 2000, 0, 150)
        self.JC.set_gains('left_ankle', 250, 0, 20)
        self.JC.send_pos_traj(self.RS.GetJointPos(), self.seq2_2, 0.5, 0.01)
        rospy.sleep(0.5)
        self.JC.reset_gains()
        self.reset()
        rospy.sleep(2)

        self.StartTime = rospy.Time.now().to_sec()

        self.JC.set_pos("left_ankle", self.SupAnkSetPoint)
        self.JC.set_gains("left_ankle", self.SupAnkGain_p, 0,
                          self.SupAnkGain_d)

        self.Leg = "right"
        self.Go = 1
        while self.Go == 1:
            TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

            if self.GlobalPos.z < 0.6 or self.GlobalPos.z > 1.4 or (
                    TimeElapsed > TimeOut and TimeOut > 0):
                # if the robot fell, stop running and return fitness
                r, p, y = self.RS._orientation.GetRPY()
                self.GlobalPos.x += 1 * math.sin(
                    self.RS.GetJointPos(self.Leg + '_hip') - p)
                return self.GlobalPos, self.ApertureStd

            self.TakeStep(self.Leg)

            self.StepsTaken += 1
            ThisAperture = abs(
                self.RS.GetJointPos('left_hip') -
                self.RS.GetJointPos('right_hip'))
            print ThisAperture
            self.ApertureMean += (ThisAperture -
                                  self.ApertureMean) / self.StepsTaken
            self.ApertureStd += ((ThisAperture - self.ApertureMean)**2 -
                                 self.ApertureStd) / self.StepsTaken

            if self.Leg == "right":
                self.Leg = "left"
            else:
                self.Leg = "right"
Beispiel #2
0
class STC_Controller(object):
    """STC_Controller"""
    def __init__(self, arg):
        super(STC_Controller, self).__init__()
        ##################################################################
        ###################### Basic Standing Pose #######################
        ##################################################################

        self.BasStndPose = zeros(28)
        self.BasStndPose[5] = 0.1
        self.BasStndPose[5 + 6] = -0.1
        self.BasStndPose[6] = self.BasStndPose[6 + 6] = -0.2
        self.BasStndPose[7] = self.BasStndPose[7 + 6] = 0.4
        self.BasStndPose[8] = self.BasStndPose[8 + 6] = -0.2
        self.BasStndPose[9] = -0.1
        self.BasStndPose[9 + 6] = 0.1
        self.BasStndPose[17] = -1.3
        self.BasStndPose[17 + 6] = 1.3
        self.BasStndPose[18] = self.BasStndPose[18 + 6] = 1.5
        self.BasStndPose[21] = 0.4
        self.BasStndPose[21 + 6] = -0.4

        self.BaseHandPose = zeros(12)
        self.BaseHandPose[1] = self.BaseHandPose[1 + 3] = self.BaseHandPose[
            1 + 6] = 1.5
        self.OpenHandPose = zeros(12)
        self.ClosedHandPose = zeros(12)
        self.ClosedHandPose[1] = self.ClosedHandPose[
            1 + 3] = self.ClosedHandPose[1 + 6] = self.ClosedHandPose[1 +
                                                                      9] = 1.5
        self.ClosedHandPose[2] = self.ClosedHandPose[
            2 + 3] = self.ClosedHandPose[2 + 6] = self.ClosedHandPose[2 +
                                                                      9] = 1.5

        ##################################################################
        ########################## INITIALIZE ############################
        ##################################################################

        self._robot_name = "atlas"
        self._jnt_names = [
            'back_lbz',
            'back_mby',
            'back_ubx',
            'neck_ay',  #3
            'l_leg_uhz',
            'l_leg_mhx',
            'l_leg_lhy',
            'l_leg_kny',
            'l_leg_uay',
            'l_leg_lax',  #9
            'r_leg_uhz',
            'r_leg_mhx',
            'r_leg_lhy',
            'r_leg_kny',
            'r_leg_uay',
            'r_leg_lax',  #15
            'l_arm_usy',
            'l_arm_shx',
            'l_arm_ely',
            'l_arm_elx',
            'l_arm_uwy',
            'l_arm_mwx',  #21
            'r_arm_usy',
            'r_arm_shx',
            'r_arm_ely',
            'r_arm_elx',
            'r_arm_uwy',
            'r_arm_mwx'
        ]  #27

        # Initialize joint commands handler
        self.JC = JointCommands_msg_handler(self._robot_name, self._jnt_names)
        self.LHC = hand_joint_controller("left")
        self.RHC = hand_joint_controller("right")

        # Initialize robot state listener
        self.RS = robot_state(self._jnt_names)
        self.MsgSub = rospy.Subscriber('/' + self._robot_name + '/atlas_state',
                                       AtlasState, self.RS_cb)
        self.OdomSub = rospy.Subscriber('/C25/publish', C25C0_ROP,
                                        self.Odom_cb)
        self.GlobalPos = 0
        self.GlobalOri = 0

        self.reset_srv = rospy.ServiceProxy('/gazebo/reset_models', Empty)

        self.TF = tf.TransformListener()

        ##################################################################
        ######################## Controller Gains ########################
        ##################################################################

        self.JC.set_gains('l_leg_uhz', 1000, 0, 50)
        self.JC.set_gains('r_leg_uhz', 1000, 0, 50)
        self.JC.set_gains('l_leg_mhx', 1000, 0, 50)
        self.JC.set_gains('r_leg_mhx', 1000, 0, 50)
        self.JC.set_gains('back_lbz', 5000, 0, 100)
        self.JC.set_gains('back_ubx', 5000, 0, 100)
        self.JC.set_gains('back_mby', 5000, 0, 100)
        self.JC.set_gains('l_arm_usy', 1000, 0, 10)
        self.JC.set_gains('r_arm_usy', 1000, 0, 10)
        self.JC.set_gains('l_arm_shx', 1000, 0, 10)
        self.JC.set_gains('r_arm_shx', 1000, 0, 10)
        self.JC.set_gains('l_arm_ely', 1000, 0, 10)
        self.JC.set_gains('r_arm_ely', 1000, 0, 10)
        self.JC.set_gains("l_arm_elx", 900, 0, 5)
        self.JC.set_gains("r_arm_elx", 900, 0, 5)
        self.JC.set_gains("l_arm_mwx", 900, 0, 5)
        self.JC.set_gains("r_arm_mwx", 900, 0, 5)
        self.JC.send_command()

    ##################################################################
    ########################### FUNCTIONS ############################
    ##################################################################

    def RS_cb(self, msg):
        self.RS.UpdateState(msg)

    def Odom_cb(self, msg):
        self.GlobalPos = msg.pose.pose.pose.position
        self.GlobalOri = msg.pose.pose.pose.orientation

    def ResetPose(self):
        self.JC.set_all_pos([0] * 28)
        self.JC.send_command()

    def reset(self):
        self.reset_srv()
        rospy.sleep(1)

        # while self.GlobalPos.z<0.9 or self.GlobalPos.z>1: #or abs(self.GlobalPos.x)>0.5:
        # # while self.GlobalPos.z<0.25 or self.GlobalPos.z>0.4: #or abs(self.GlobalPos.x)>0.5:
        #     self.reset_srv()
        #     rospy.sleep(1)

    def current_ypr(self):
        try:
            quat = copy(self.GlobalOri)
            (r, p, y) = tf.transformations.euler_from_quaternion(
                [quat.x, quat.y, quat.z, quat.w])
        except:
            (r, p, y) = [0, 0, 0]
        return (y, p, r)

    def DeltaAngle(self, DesAngle, CurAngle):
        Delta = DesAngle - CurAngle
        if Delta > math.pi:
            Delta -= 2 * math.pi
        elif Delta < -math.pi:
            Delta += 2 * math.pi
        return Delta

    def SeqWithBalance(self, pos1, pos2, T, dt, ref_link='/l_foot', COMref=0):
        if len(pos1) == len(pos2) == len(self._jnt_names):
            N = ceil(T / dt)
            pos1 = array(pos1)
            pos2 = array(pos2)

            init_com, rot = self.TF.lookupTransform('/com', ref_link,
                                                    rospy.Time(0))
            if COMref != 0:
                COMref0 = [init_com[0], init_com[1]]
                init_com = [0] * 2

            for ratio in linspace(0, 1, N):
                cur_com, rot = self.TF.lookupTransform('/com', ref_link,
                                                       rospy.Time(0))
                interpCommand = (1 - ratio) * pos1 + ratio * pos2

                if COMref != 0:
                    init_com[0] = (1 - ratio) * COMref0[0] + ratio * COMref[0]
                    init_com[1] = (1 - ratio) * COMref0[1] + ratio * COMref[1]

                # Balance torso
                back_mby_pos = interpCommand[1] + 3 * (cur_com[0] -
                                                       init_com[0])
                back_ubx_pos = interpCommand[2] + 2 * (cur_com[1] -
                                                       init_com[1])

                self.JC.set_all_pos([float(x) for x in interpCommand])
                self.JC.set_pos("back_mby", back_mby_pos)
                self.JC.set_pos("back_ubx", back_ubx_pos)

                self.JC.send_command()
                rospy.sleep(dt)

        else:
            print 'position command legth doest fit'

    def Run(self, TimeOut=0):
        rospy.sleep(0.1)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1, 0.01)

    def SwingIn(self):
        # Raise arms above head
        pos = copy(self.BasStndPose)
        pos[3] = 1
        pos[17] = 1.8
        pos[17 + 6] = -1.8
        pos[6] = pos[6 + 6] = 0
        pos[7] = pos[7 + 6] = 0.1
        pos[8] = pos[8 + 6] = -0.05
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.6, 0.005)

        pos[8] = pos[8 + 6] = -0.2
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)

        rospy.sleep(0.5)

        # "Grasp" top pole with wrists
        # Lift own weight by flexing elbows
        pos[8] = pos[8 + 6] = 0.6
        pos[19] = 0.8
        pos[19 + 6] = -0.8
        pos[21] = 1.5
        pos[21 + 6] = -1.5
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)

        # Lift legs of ground and flex pelvis+hips dynamically
        pos[1] = 1.5
        pos[5] = pos[5 + 6] = 0
        pos[6] = pos[6 + 6] = -1.6
        pos[7] = pos[7 + 6] = 2.4
        pos[8] = pos[8 + 6] = -0.2
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        pos[8] = pos[8 + 6] = -0.6
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)

        # Check current time
        T0 = rospy.Time.now().to_sec()
        while True:
            # Get current orientation
            r, p, y = self.current_ypr()

            if p < -1.3:
                print "Wheeeee..."
                pos[7] = pos[7 + 6] = 0
                self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.15, 0.005)
                break
            elif p > -0.5 and rospy.Time.now().to_sec() - T0 > 4:
                # This is taking too long, extend and flex again
                pos[6] = pos[6 + 6] = -0.7
                pos[7] = pos[7 + 6] = 1.3
                self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.005)
                pos[6] = pos[6 + 6] = -1.6
                pos[7] = pos[7 + 6] = 2.4
                pos[8] = pos[8 + 6] = -0.2
                self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)
                T0 = rospy.Time.now().to_sec()
            else:
                rospy.sleep(0.005)

        print "...eeeee!"
        pos[1] = 0
        pos[6] = pos[6 + 6] = 0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.15, 0.005)

        rospy.sleep(0.5)
        # "Catch" seat with heels and pull
        pos[7] = pos[7 + 6] = 1.2
        pos[8] = pos[8 + 6] = 0.7
        pos[6 + 6] = -1.5
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.6, 0.005)
        rospy.sleep(0.5)
        # Release right hand to "hang on the side"
        pos[21 + 6] = 0.5
        pos[5] = -0.3
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        # Extend left arm to lower robot smoothly
        pos[16] = -1.5
        pos[17] = 0.0
        pos[18] = 1.9
        pos[19] = 0
        pos[20] = 1.5
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        # Release left hand
        pos[21] = -0.5
        pos[8 + 6] = -0.1
        self.JC.set_gains('r_leg_uay', 30, 0, 1, set_default=False)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)
        rospy.sleep(0.3)

    def GetHandsIn(self):
        pos = copy(self.BasStndPose)
        pos[1] = 0
        pos[3] = 1
        pos[5] = pos[5 + 6] = 0
        pos[6] = 0
        pos[6 + 6] = -1.6
        pos[7] = pos[7 + 6] = 1.2
        pos[8] = pos[8 + 6] = 0.7
        pos[8 + 6] = -0.1
        pos[16] = -1.5
        pos[17] = 0.0
        pos[18] = 1.9
        pos[19] = 0
        pos[20] = 1.5
        pos[21] = 0
        pos[17 + 6] = -1.8
        pos[19 + 6] = -0.8
        pos[21 + 6] = 0.5

        # Bring left arm into the car
        pos[16] = -1.3
        pos[17] = -1.2
        pos[18] = 0.6
        pos[19] = 1.8
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)

        # Rotate right arm, preparing to enter the car
        pos[16 + 6] = -0.3
        pos[17 + 6] = -0.4
        pos[18 + 6] = 0
        pos[19 + 6] = -2
        pos[21 + 6] = -1.5
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1, 0.005)
        # Rotate arm at shoulder to help robot lay flat on the seat
        # (with help from right leg)
        pos[2] = 0.2
        pos[4 + 6] = -0.5
        pos[16 + 6] = 1.0
        pos[17 + 6] = -0.1
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        rospy.sleep(0.3)
        # Rotate right arm into car
        pos[16 + 6] = -1.5
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.7, 0.005)
        pos[17 + 6] = 1.3
        pos[18 + 6] = 0.4
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.005)
        # Straighten hands
        pos[16] = -1.35
        pos[16 + 6] = -1.35
        pos[17] = -1.8
        pos[17 + 6] = 1.3
        pos[18] = pos[18 + 6] = 0.3
        pos[19] = 0.2
        pos[19 + 6] = -0.2
        pos[20] = pos[20 + 6] = 0
        pos[21] = pos[21 + 6] = 0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        self.JC.reset_gains(8 + 6)

    def SitUp(self):
        pos = copy(self.BasStndPose)
        pos[0] = 0
        pos[2] = 0.5
        pos[1] = 0
        pos[3] = 1
        pos[5] = pos[5 + 6] = 0
        pos[5 + 6] = -0.5
        pos[6] = pos[6 + 6] = 0
        pos[7] = pos[7 + 6] = 1.6
        pos[8] = pos[8 + 6] = 0
        pos[16] = -1.35
        pos[16 + 6] = -1.45
        pos[17] = -1.8
        pos[17 + 6] = 1.8
        pos[18] = 0.3
        pos[18 + 6] = 0.4
        pos[19] = 0.2
        pos[19 + 6] = -0.2
        pos[20] = pos[20 + 6] = 0
        pos[21] = pos[21 + 6] = 0

        # Extend knees
        pos[7] = 0
        pos[7 + 6] = 0.7
        pos[8 + 6] = -0.7
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.3, 0.005)

        # Flex torso and hips
        # Move right arm up so it won't interfere with the st. wheel
        pos[1] = 1.5
        pos[5] = 0.5
        pos[6] = pos[6 + 6] = -0.7
        # pos[7+6] = 1.4
        pos[17] = -0.7
        pos[16 + 6] = -1.1
        pos[17 + 6] = -0.4
        pos[19 + 6] = -1
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        rospy.sleep(0.3)

        # Use left arm to push on frame to help sit up
        # Push on dashboard with right hand
        pos[6] = -1.2
        pos[7] = 1.3  #1.5
        pos[8] = -0.7
        pos[4 + 6] = -0.5
        pos[5 + 6] = 0.1
        pos[7 + 6] = 1.2
        # pos[7] = 1.7
        pos[17] = 1.1  # 0.9
        pos[19] = 0.6  # 0.8 # 1.3
        pos[16 + 6] = 0.5
        pos[17 + 6] = 0.2
        pos[18 + 6] = 1.0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)

        # "Numb" legs
        # self.JC.set_gains('l_leg_lhy',10,0,10,set_default=False)
        self.JC.set_gains('l_leg_kny', 10, 0, 10, set_default=False)
        self.JC.set_gains('l_leg_lax', 30, 0, 1, set_default=False)
        self.JC.set_gains('r_leg_kny', 10, 0, 10, set_default=False)
        self.JC.set_gains('r_leg_lax', 30, 0, 1, set_default=False)

        # Straighten most body joints into sitting position
        pos[0] = 0.5
        pos[2] = 0
        pos[1] = 0.2
        pos[4] = -0.2
        pos[5] = 0
        pos[5 + 6] = 0
        pos[6] = -1.9  #-1.7 # -1.6
        pos[6 + 6] = -1.9
        pos[7] = 1.5
        pos[7 + 6] = 2
        pos[17] = 0.6
        pos[19] = 0.8  # 1.3
        pos[16 + 6] = -0.4
        pos[17 + 6] = 0.5
        pos[19 + 6] = -0.6
        self.JC.set_gains('r_leg_lax', 30, 0, 1, set_default=False)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1.5, 0.005)
        rospy.sleep(0.5)
        self.JC.set_gains('r_leg_kny', 30, 0, 1, set_default=False)

        # Overcompensate with back_y to make sure the robot sits
        # pos[1] = -0.3
        # pos[2] = 0.3
        pos[17] = -0.9
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        rospy.sleep(0.5)

        # # Return back_y to 0
        # # pos[0] = 0
        # pos[1] = 0
        # pos[2] = 0
        # pos[6+6] = -1.5
        # pos[7+6] = 1.8
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.5,0.005)

        # self.JC.reset_gains(6)
        self.JC.reset_gains(7)
        self.JC.reset_gains(9)
        self.JC.reset_gains(7 + 6)
        self.JC.reset_gains(9 + 6)

        ######################################
        pos[0] = 0
        pos[1] = -0.1
        pos[4] = 0.2
        pos[4 + 6] = -0.2
        pos[5] = pos[5 + 6] = 0
        pos[6] = pos[6 + 6] = -1.4
        pos[7] = pos[7 + 6] = 1.4
        pos[19] = 1.5
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        pos[7] = 1.2
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        pos[18] = 0.7
        pos[19] = 1.1
        pos[19 + 6] = -0.9
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)

        ######################################

        # Bend right arm to grab gear stick
        # pos[0] = -0.5
        # pos[16+6] = 0
        # pos[17+6] = 0.2
        # pos[18+6] = 1.0
        # pos[19+6] = -1.7
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.5,0.005)

    def RotateToPedals(self, DesOri):
        pos = copy(self.BasStndPose)
        pos[0] = 0  #-0.5 #0.2
        pos[1] = 0
        pos[2] = 0
        pos[3] = 1
        pos[4] = -0.2
        pos[5] = 0
        pos[6] = -1.6
        pos[7] = 1.8
        pos[8] = 0.7
        pos[4 + 6] = -0.5
        pos[5 + 6] = 0
        pos[6 + 6] = -1.5
        pos[7 + 6] = 1.8
        pos[8 + 6] = -0.7
        pos[16] = -1.35
        pos[17] = -0.9
        pos[18] = 0.3
        pos[19] = 1.3
        pos[16 + 6] = -0.6
        pos[17 + 6] = 0.2
        pos[18 + 6] = 1.0
        pos[19 + 6] = -0.4
        # pos[16+6] = 0
        # pos[17+6] = 0.2
        # pos[18+6] = 1.0
        # pos[19+6] = -1.7
        pos[20] = pos[20 + 6] = 0
        pos[21] = pos[21 + 6] = 0

        # Lower right arm and rotate it inward
        # Rotate left arm outward
        # Straighten legs
        # pos[4] = pos[4+6] = 0
        pos[5] = pos[5 + 6] = 0
        # pos[6] = pos[6+6] = -1.8
        pos[8] = pos[8 + 6] = -0.7
        pos[16] = -1.1
        pos[16 + 6] = -1.1
        pos[17 + 6] = 0.6
        pos[19 + 6] = -0.8
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)

        # Push back with legs
        pos[16 + 6] = -0.9
        pos[7] = 1.3
        pos[7 + 6] = 1.4
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1, 0.005)
        pos[0] = 0.2
        pos[4] = 0.2
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.4, 0.005)
        pos[0] = 0
        pos[4] = 0
        pos[7] = pos[7 + 6] = 1.7
        pos[6] = -1.4
        # pos[8] = 0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.005)
        # pos[6] = -1.4
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.4,0.005)

        # Insert left arm
        pos[17] = -1.5
        pos[19] = 2
        pos[21] = 1.0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.005)
        pos[0] = -0.4
        pos[16 + 6] = -1.3
        pos[17 + 6] = 0.8
        pos[19 + 6] = -0.6
        pos[16] = -1.6
        pos[18] = 0.8
        pos[21] = 0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.005)

        # Extend and rotate left arm outward
        pos[16] = -1.5
        pos[17] = -1.1
        pos[18] = 1.0
        pos[19] = 1.0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.005)
        pos[16] = -1.3
        pos[18] = 0.3
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.005)

        # Rotate in place towards target
        self.JC.set_gains('r_leg_lax', 30, 0, 1, set_default=False)
        self.JC.set_gains('l_leg_lax', 30, 0, 1, set_default=False)
        #    # Check current time
        #    T0 = rospy.Time.now().to_sec()
        #    while True:
        #        # Get current orientation
        #        r,p,y = STC.current_ypr()
        #        print DesOri, y

        #        # Check if more than 15 seconds elapsed
        #        if rospy.Time.now().to_sec()-T0>10:
        #            if abs(self.DeltaAngle(DesOri,y))>0.1:
        #                self.RotateWithLegs(-self.DeltaAngle(DesOri,y),pos)
        #            else:
        #                break

        #            if rospy.Time.now().to_sec()-T0>20:
        #                print "Rotate to pedals: FAIL!"
        #                print "Giving up... Please try again"
        #                #break
        #        else:
        #            if abs(self.DeltaAngle(DesOri,y))>0.1:
        #                pass
        # self.RotateWithLegs(self.DeltaAngle(DesOri,y),pos)

        #        rospy.sleep(0.01)
        pos[0] = 0.2
        pos[2] = -0.2
        pos[4] = 0.3
        pos[4 + 6] = -0.3
        pos[5 + 6] = -0.2
        pos[6 + 6] = -1.7
        pos[7] = 1.5
        pos[8] = 0.7
        pos[8 + 6] = -0.7
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
        self.JC.reset_gains(9)
        self.JC.reset_gains(9 + 6)

        # Full gas ahead
        self.JC.set_gains('r_leg_uay', 30, 0, 1, set_default=False)
        pos[7 + 6] = 1.0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)

    def RotateWithLegs(self, Delta, pos):
        # # Push back with legs
        # pos[7] = pos[7+6] = 0.4
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.7,0.005)
        # pos[7] = pos[7+6] = 1.7
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.5,0.005)

        if Delta > 0:
            print "Turning right..."
            # Rotate torso with help from the legs
            pos[6] = pos[6 + 6] = -1.9
            pos[7] = pos[7 + 6] = 1.8
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)
            pos[0] = 0.5
            pos[4] = 0  # NEW
            pos[4 + 6] = 0  # NEW
            # pos[16+6] = -1.3
            pos[17 + 6] = 0.8
            pos[18 + 6] = 1.2
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
            pos[6] = -1.6
            pos[7] = 1.7
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)
            pos[0] = 0
            pos[4] = 0.3
            pos[4 + 6] = -0.3
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
            pos[6 + 6] = -1.6
            pos[7 + 6] = 1.7
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)
        else:
            print "Turning left..."
            # Rotate torso with help from the legs
            pos[6] = pos[6 + 6] = -1.8
            pos[7] = pos[7 + 6] = 1.8
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)
            pos[0] = -0.5
            pos[4] = 0  # NEW
            pos[4 + 6] = 0  # NEW
            # pos[16+6] = -1.3
            pos[17 + 6] = 0.8
            pos[18 + 6] = 1.2
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
            pos[6 + 6] = -1.6
            pos[7 + 6] = 1.7
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)
            pos[0] = 0
            pos[4] = 0.3
            pos[4 + 6] = -0.3
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5, 0.005)
            pos[6] = -1.6
            pos[7] = 1.7
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.005)
Beispiel #3
0
class Fivel_Controller(object):
    """Fivel_Controller"""
    def __init__(self, arg):
        super(Fivel_Controller, self).__init__()

        ##################################################################
        ######################## GAIT PARAMETERS #########################
        ##################################################################
        if len(arg)<10:
            self.SwingEff0 = -1.2             # Default swing effort
            self.SupAnkSetPoint = -0.04       # Setpoint for support ankle
            self.SupAnkGain_p = 30            # Support ankle gain p
            self.SupAnkGain_d = 10            # Support ankle gain d
            self.SwingEffDur = 0.50           # Duration of swing effort
            self.FootExtEff = 1.0             # Foot extension effort
            self.FootExtDur = 0.22            # Foot extension duration
            self.ToeOffEff = 17.65            # Toe-off effort
            self.ToeOffDur = 0.24             # Toe-off duration
            self.DesTorsoAng = -0.055         # Desired torso angle
        else:
            self.SwingEff0 = arg[0]           # Default swing effort
            self.SupAnkSetPoint = arg[1]      # Setpoint for support ankle
            self.SupAnkGain_p = arg[2]        # Support ankle gain p
            self.SupAnkGain_d = arg[3]        # Support ankle gain d
            self.SwingEffDur = arg[4]         # Duration of swing effort
            self.FootExtEff = arg[5]          # Foot extension effort
            self.FootExtDur = arg[6]          # Foot extension duration
            self.ToeOffEff = arg[7]           # Toe-off effort
            self.ToeOffDur = arg[8]           # Toe-off duration
            self.DesTorsoAng = arg[9]         # Desired torso angle
        
        self.StepsTaken = 0
        self.ApertureMean = 0
        self.ApertureStd = 0
        self.GlobalPos = 0

        self.reset_srv = rospy.ServiceProxy('/gazebo/reset_models', Empty)

        ##################################################################
        ###################### RAISE FOOT SEQUENCE #######################
        ##################################################################

        self.pos1=zeros(4)

        # "left_hip","left_ankle","right_hip","right_ankle"
        #      0           1           2            3     

        # Sequence 1 for raising the left foot (from home position)
        self.seq1_1=copy(self.pos1)
        self.seq1_1[3] = -0.05
        self.seq1_1[0] = 0.25
        self.seq1_1[1] = -0.25

        self.seq1_2=copy(self.seq1_1)
        self.seq1_2[1] = -1.49

        # Sequence 2 for raising the right foot (from home position)
        self.seq2_1=copy(self.pos1)
        self.seq2_1[0] = 0.04
        self.seq2_1[1] = -0.04
        self.seq2_1[2] = 0.25
        self.seq2_1[3] = -0.25

        self.seq2_2=copy(self.seq2_1)
        self.seq2_2[3] = -1.49

        ##################################################################
        ########################## INITIALIZE ############################
        ##################################################################

        self.RobotName = "ghost_fivel"
        self.JointNames = ["left_hip","left_ankle","right_hip","right_ankle"]

        # Initialize joint commands handler
        self.JC = JointCommands_msg_handler(self.RobotName,self.JointNames)

        # Initialize robot state listener
        self.RS = robot_state(self.JointNames)
        self.MsgSub = rospy.Subscriber('/'+self.RobotName+'/robot_state',RobotState,self.RS_cb)
        self.OdomSub = rospy.Subscriber('/ground_truth_odom',Odometry,self.Odom_cb)


    ##################################################################
    ########################### FUNCTIONS ############################
    ##################################################################

    # def RaiseLeg(self,which):
    #     if which == "inner":
    #         self.JC.send_pos_traj(self.pos1,self.seq1_1,1,0.01)
    #         rospy.sleep(0.2)
    #         self.JC.send_pos_traj(self.seq1_1,self.seq1_2,1,0.01)
    #         self.JC.set_eff('inner_ankle',-5)
    #         self.JC.send_pos_traj(self.seq1_2,self.seq1_3,1,0.01)
    #         rospy.sleep(0.2)
    #     if which == "outer":
    #         self.JC.send_pos_traj(self.pos1,self.seq2_1,2,0.01)
    #         rospy.sleep(0.2)
    #         self.JC.send_pos_traj(self.seq2_1,self.seq2_2,2,0.01)
    #         self.JC.set_eff('left_ankle',-5)
    #         self.JC.set_eff('right_ankle',-5)
    #         self.JC.send_command()
    #         #self.JC.send_pos_traj(self.seq2_2,self.seq2_3,1,0.01)
    #         rospy.sleep(0.2)

    def StandOn(self,leg,setpoint,time,dt):
        StartTime = rospy.Time.now().to_sec()
        while rospy.Time.now().to_sec() - StartTime < time:
            # Apply closed loop torque for upright torso
            hip_eff = self.BalanceCmd(leg,setpoint)

            self.JC.set_eff(leg+'_hip',hip_eff)
            self.JC.send_command()
            rospy.sleep(dt)

    def BalanceCmd(self,which,setpoint):
        ank_pos = self.RS.GetJointPos(which+"_ankle")
        hip_pos = self.RS.GetJointPos(which+"_hip")
        ank_vel = self.RS.GetJointVel(which+"_ankle")
        hip_vel = self.RS.GetJointVel(which+"_hip")

        torso_angle = ank_pos + hip_pos
        torso_angvel = ank_vel + hip_vel

        # Apply closed loop torque for upright torso
        hip_eff = 200*(setpoint-torso_angle) + 50*(0-torso_angvel)
        return hip_eff


    def TakeStep(self,which):
        #Aperture = abs(self.RS.GetJointPos('left_hip')-self.RS.GetJointPos('right_hip'))
        SwingEff = self.SwingEff0 #+ self.kAp * (self.DesAp - Aperture)
        DesTorsoAng = self.DesTorsoAng
        # if which == "left":
        #     DesTorsoAng = self.DesTorsoAng - 0.1*self.RS.GetJointPos('right_hip')
        # if which == "right":
        #     DesTorsoAng = self.DesTorsoAng - 0.1*self.RS.GetJointPos('left_hip')

        if which == "left":
            # Swing leg 0.4 sec and incline stance leg
            self.JC.set_eff("left_hip",SwingEff)
            self.JC.send_command()
            self.StandOn("right",DesTorsoAng,self.SwingEffDur,0.005)
            # Straighten ankle and free swing
            self.JC.set_eff("left_ankle",self.FootExtEff)
            self.JC.set_eff("left_hip",0)
            self.JC.send_command()
            self.StandOn("right",DesTorsoAng,self.FootExtDur,0.005)
            # Toe off
            self.JC.set_eff("left_ankle",0)
            self.JC.set_pos("left_ankle",self.SupAnkSetPoint)
            self.JC.set_gains("left_ankle",self.SupAnkGain_p,0,self.SupAnkGain_d)
            self.JC.set_eff("right_ankle",self.ToeOffEff)
            self.JC.send_command()
            self.StandOn("left",DesTorsoAng,self.ToeOffDur,0.005)
            # Bend foot for clearance
            self.JC.set_eff("right_ankle",-2)
            self.JC.set_eff("right_hip",0)
            self.JC.send_command()

        if which == "right":
            # Swing leg 0.4 sec and incline stance leg
            self.JC.set_eff("right_hip",SwingEff)
            self.JC.send_command()
            self.StandOn("left",DesTorsoAng,self.SwingEffDur,0.005)
            # Straighten ankle and free swing
            self.JC.set_eff("right_ankle",self.FootExtEff)
            self.JC.set_eff("right_hip",0)
            self.JC.send_command()
            self.StandOn("left",DesTorsoAng,self.FootExtDur,0.005)
            # Toe off
            self.JC.set_eff("right_ankle",0)
            self.JC.set_pos("right_ankle",self.SupAnkSetPoint)
            self.JC.set_gains("right_ankle",self.SupAnkGain_p,0,self.SupAnkGain_d)
            self.JC.set_eff("left_ankle",self.ToeOffEff)
            self.JC.send_command()
            self.StandOn("right",DesTorsoAng,self.ToeOffDur,0.005)
            # Bend foot for clearance
            self.JC.set_eff("left_ankle",-2)
            self.JC.set_eff("left_hip",0)
            self.JC.send_command()

    # def TakeStep(self,which):
    #     TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

    #     if TimeElapsed<60:
    #         Aperture = abs(self.RS.GetJointPos('left_hip'))
    #         self.SwingEff = self.SwingEff0 + self.kAp*(self.DesAp - Aperture)
    #         #self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)
            
    #         if which == 'outer':
				# Aperture = self.RS.GetJointPos('left_hip')
    #         else:
				# Aperture = -self.RS.GetJointPos('left_hip')
    #         self.ToeOffEff = self.ToeOffEff0 - 1*Aperture
    #         #self.SupAnkSetPoint = -0.02 + 0.03 * Aperture
			
    #         print Aperture
    #     else:
    #         Aperture = abs(self.RS.GetJointPos('left_hip'))
    #         #self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)
    #         #print self.ToeOffEff
    #         #print self.SwingEff
    #     if which == "outer":
    #         # Toe off
    #         self.JC.set_gains('inner_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
    #         self.JC.set_pos('inner_ankle',self.SupAnkSetPoint)
    #         self.JC.set_eff('left_ankle',self.ToeOffEff)
    #         self.JC.set_eff('right_ankle',self.ToeOffEff)
    #         self.JC.set_eff('left_hip',0)
    #         self.JC.set_eff('right_hip',0)
    #         self.JC.send_command()
    #         rospy.sleep(self.ToeOffDur)
    #         # Raise feet and swing
    #         self.JC.set_eff('left_ankle',-3)
    #         self.JC.set_eff('right_ankle',-3)
    #         self.JC.set_eff('left_hip',-self.SwingEff)
    #         self.JC.set_eff('right_hip',-self.SwingEff)
    #         self.JC.send_command()
    #         rospy.sleep(self.TotSwingDur-self.FootExtDur-self.FreeSwingDur)
    #         # Lower feet
    #         self.JC.set_eff('left_ankle',self.FootExtEff)
    #         self.JC.set_eff('right_ankle',self.FootExtEff)
    #         self.JC.send_command()
    #         rospy.sleep(self.FootExtDur)
    #         # End swing
    #         self.JC.set_eff('left_hip',0)
    #         self.JC.set_eff('right_hip',0)
    #         self.JC.set_eff('left_ankle',0)
    #         self.JC.set_eff('right_ankle',0)
    #         self.JC.send_command()
    #         rospy.sleep(self.FreeSwingDur)

    #     if which == "inner":
    #         # Toe off
    #         self.JC.set_gains('left_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
    #         self.JC.set_gains('right_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
    #         self.JC.set_pos('left_ankle',self.SupAnkSetPoint)
    #         self.JC.set_pos('right_ankle',self.SupAnkSetPoint)
    #         self.JC.set_eff('inner_ankle',self.ToeOffEff)
    #         self.JC.set_eff('left_hip',0)
    #         self.JC.set_eff('right_hip',0)
    #         self.JC.send_command()
    #         rospy.sleep(self.ToeOffDur)
    #         # Raise feet and swing
    #         self.JC.set_eff('inner_ankle',-3)
    #         self.JC.set_eff('left_hip',self.SwingEff)
    #         self.JC.set_eff('right_hip',self.SwingEff)
    #         self.JC.send_command()
    #         rospy.sleep(self.TotSwingDur-self.FootExtDur-self.FreeSwingDur)
    #         # Lower feet
    #         self.JC.set_eff('inner_ankle',self.FootExtEff)
    #         self.JC.send_command()
    #         rospy.sleep(self.FootExtDur)
    #         # End swing
    #         self.JC.set_eff('left_hip',0)
    #         self.JC.set_eff('right_hip',0)
    #         self.JC.set_eff('inner_ankle',0)
    #         self.JC.send_command()
    #         rospy.sleep(self.FreeSwingDur)

    def ResetPose(self):
        self.JC.set_pos('left_hip',0)
        self.JC.set_pos('left_ankle',0)
        self.JC.set_pos('right_hip',0)
        self.JC.set_pos('right_ankle',0)
        self.JC.send_command()

    def RS_cb(self,msg):
        self.RS.UpdateState(msg)

    def Odom_cb(self,msg):
        self.GlobalPos = msg.pose.pose.position

    def reset(self):
        self.reset_srv()
        rospy.sleep(1.5)

        while self.GlobalPos.z<0.97 or self.GlobalPos.z>1.03 or abs(self.GlobalPos.x)>0.5:
            self.reset_srv()
            rospy.sleep(1)

    def Run(self,TimeOut = 0):
        rospy.sleep(0.1)

        # Start with left leg raised
        # self.JC.send_pos_traj(self.RS.GetJointPos(),self.seq1_2,0.5,0.01) 
        # rospy.sleep(0.5)
        # self.reset()
        # self.StandOn("right",10,0.005)

        # Start with right leg raised
        self.JC.set_gains('right_hip',2000,0,150)
        self.JC.set_gains('left_hip',2000,0,150)
        self.JC.set_gains('left_ankle',250,0,20)
        self.JC.send_pos_traj(self.RS.GetJointPos(),self.seq2_2,0.5,0.01) 
        rospy.sleep(0.5)
        self.JC.reset_gains()
        self.reset()
        rospy.sleep(2)

        self.StartTime = rospy.Time.now().to_sec()

        self.JC.set_pos("left_ankle",self.SupAnkSetPoint)
        self.JC.set_gains("left_ankle",self.SupAnkGain_p,0,self.SupAnkGain_d)

        self.Leg = "right"
        self.Go=1
        while self.Go == 1:
            TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

            if self.GlobalPos.z<0.6 or self.GlobalPos.z>1.4 or (TimeElapsed>TimeOut and TimeOut>0):
                # if the robot fell, stop running and return fitness
                r,p,y = self.RS._orientation.GetRPY()
                self.GlobalPos.x += 1*math.sin(self.RS.GetJointPos(self.Leg+'_hip')-p)
                return self.GlobalPos, self.ApertureStd

            self.TakeStep(self.Leg)

            self.StepsTaken += 1
            ThisAperture = abs(self.RS.GetJointPos('left_hip')-self.RS.GetJointPos('right_hip'))
            print ThisAperture
            self.ApertureMean += (ThisAperture-self.ApertureMean) / self.StepsTaken
            self.ApertureStd += ((ThisAperture-self.ApertureMean)**2 - self.ApertureStd) / self.StepsTaken

            if self.Leg == "right":
                self.Leg = "left"
            else:
                self.Leg = "right"
Beispiel #4
0
class Fivel_Controller(object):
    """Fivel_Controller"""
    def __init__(self, arg):
        super(Fivel_Controller, self).__init__()

        ##################################################################
        ######################## GAIT PARAMETERS #########################
        ##################################################################
        if len(arg)<12:
            self.SwingEff0 = 0.44        # Default swing effort
            self.kAp = 3                 # Aperture feedback gain
            self.DesAp = 0.4             # Desired aperture
            self.SupAnkGain_p = 10       # Ankle gain p during support
            self.SupAnkGain_d = 10       # Ankle gain d during support
            self.SupAnkSetPoint = -0.02  # Support ankle set point
            self.ToeOffEff0 = 3.5        # Toe-off effort
            self.ToeOffDur = 0.20        # Toe-off duration
            self.TotSwingDur = 0.85      # Swing duration
            self.FootExtEff = 0.75       # Foot extension effort (right before touch down)
            self.FootExtDur = 0.18       # Foot extension duration
            self.FreeSwingDur = 0.15     # Swing duration
            
            self.SwingEff = self.SwingEff0
            self.ToeOffEff = self.ToeOffEff0
        else:
            self.SwingEff0 = arg[0]      # Default swing effort
            self.kAp = arg[1]            # Aperture feedback gain
            self.DesAp = arg[2]          # Desired aperture
            self.SupAnkGain_p = arg[3]   # Ankle gain p during support
            self.SupAnkGain_d = arg[4]   # Ankle gain d during support
            self.SupAnkSetPoint = arg[5] # Support ankle set point
            self.ToeOffEff0 = arg[6]      # Toe-off effort
            self.ToeOffDur = arg[7]      # Toe-off duration
            self.TotSwingDur = arg[8]    # Swing duration
            self.FootExtEff = arg[9]     # Foot extension effort (right before touch down)
            self.FootExtDur = arg[10]    # Foot extension duration
            self.FreeSwingDur = arg[11]  # Swing duration
            
            self.SwingEff = self.SwingEff0
            self.ToeOffEff = self.ToeOffEff0
        
        self.StepsTaken = 0
        self.ApertureMean = 0
        self.ApertureStd = 0
        self.GlobalPos = 0

        self.reset_srv = rospy.ServiceProxy('/gazebo/reset_models', Empty)

        ##################################################################
        ###################### RAISE FOOT SEQUENCE #######################
        ##################################################################

        self.pos1=zeros(6)

        # "inner_hip","inner_ankle","left_hip","left_ankle","right_hip","right_ankle"
        #      0            1           2           3            4            5

        # Sequence 1 for raising the inner foot (from home position)
        self.seq1_1=copy(self.pos1)
        self.seq1_1[3] = self.seq1_1[5] = -0.03
        self.seq1_1[0] = 0.25
        self.seq1_1[1] = -0.25

        self.seq1_2=copy(self.seq1_1)
        self.seq1_2[1] = -1.49

        # Sequence 2 for raising the outer feet (from home position)
        self.seq2_1=copy(self.pos1)
        self.seq2_1[1] = -0.03
        self.seq2_1[2] = self.seq2_1[4] = 0.25
        self.seq2_1[3] = self.seq2_1[5] = -0.25

        self.seq2_2=copy(self.seq2_1)
        self.seq2_2[3] = self.seq2_2[5] = -1.49
        #self.seq2_2[1] = self.seq2_2[3] = -0.16
        #self.seq2_2[1] = 0.06

        #self.seq2_3=copy(self.seq2_2)
        #self.seq2_3[1] = self.seq2_3[3] = -0.1

        ##################################################################
        ########################## INITIALIZE ############################
        ##################################################################
        self._robot_name = "five_link_walker"
        self._jnt_names = ["inner_hip","inner_ankle","left_hip","left_ankle","right_hip","right_ankle"]
        # Initialize joint commands handler
        self.JC = JointCommands_msg_handler(self._robot_name,self._jnt_names)

        # Initialize robot state listener
        self.RS = robot_state(self._jnt_names)
        self.MsgSub = rospy.Subscriber('/'+self._robot_name+'/robot_state',RobotState,self.RS_cb)
        self.OdomSub = rospy.Subscriber('/ground_truth_odom',Odometry,self.Odom_cb)


    ##################################################################
    ########################### FUNCTIONS ############################
    ##################################################################

    def RaiseLeg(self,which):
        if which == "inner":
            self.JC.send_pos_traj(self.pos1,self.seq1_1,1,0.01)
            rospy.sleep(0.2)
            self.JC.send_pos_traj(self.seq1_1,self.seq1_2,1,0.01)
            self.JC.set_eff('inner_ankle',-5)
            self.JC.send_pos_traj(self.seq1_2,self.seq1_3,1,0.01)
            rospy.sleep(0.2)
        if which == "outer":
            self.JC.send_pos_traj(self.pos1,self.seq2_1,2,0.01)
            rospy.sleep(0.2)
            self.JC.send_pos_traj(self.seq2_1,self.seq2_2,2,0.01)
            self.JC.set_eff('left_ankle',-5)
            self.JC.set_eff('right_ankle',-5)
            self.JC.send_command()
            #self.JC.send_pos_traj(self.seq2_2,self.seq2_3,1,0.01)
            rospy.sleep(0.2)

    def StandOn(self,leg,time,dt):
        StartTime = rospy.Time.now().to_sec()
        while rospy.Time.now().to_sec() - StartTime < time:
            if leg == "inner":
                # Apply closed loop torque for upright torso
                hip_eff = self.BalanceCmd(leg)

                self.JC.set_eff('inner_hip',hip_eff)
                self.JC.send_command()
                rospy.sleep(dt)

    def BalanceCmd(self,which):
        if which == "inner":
            # Get angles
            ia_pos = self.RS.GetJointPos("inner_ankle")
            ih_pos = self.RS.GetJointPos("inner_hip")
            ia_vel = self.RS.GetJointVel("inner_ankle")
            ih_vel = self.RS.GetJointVel("inner_hip")

            torso_angle = ia_pos + ih_pos
            torso_angvel = ia_vel + ih_vel

            # Apply closed loop torque for upright torso
            hip_eff = 200*(0-torso_angle) + 50*(0-torso_angvel)
            return hip_eff
        if which == "outer":
            # Get angles
            la_pos = self.RS.GetJointPos("left_ankle")
            lh_pos = self.RS.GetJointPos("left_hip")
            la_vel = self.RS.GetJointVel("left_ankle")
            lh_vel = self.RS.GetJointVel("left_hip")

            ra_pos = self.RS.GetJointPos("right_ankle")
            rh_pos = self.RS.GetJointPos("right_hip")
            ra_vel = self.RS.GetJointVel("right_ankle")
            rh_vel = self.RS.GetJointVel("right_hip")

            ltorso_angle = la_pos + lh_pos
            ltorso_angvel = la_vel + lh_vel

            rtorso_angle = ra_pos + rh_pos
            rtorso_angvel = ra_vel + rh_vel

            # Apply closed loop torque for upright torso
            lhip_eff = 200*(0-ltorso_angle) + 50*(0-ltorso_angvel)
            rhip_eff = 200*(0-rtorso_angle) + 50*(0-rtorso_angvel)
            return lhip_eff,rhip_eff


    def TakeFirstStep(self,which):
        if which == "inner":
            self.JC.set_eff('left_ankle',-1)
            self.JC.set_eff('right_ankle',-1)
            self.JC.set_eff('left_hip',3.5)
            self.JC.set_eff('right_hip',3.5)
            self.JC.set_eff('inner_ankle',0)
            self.JC.send_command()
            rospy.sleep(0.67)
            self.JC.set_eff('left_ankle',0.5)
            self.JC.set_eff('right_ankle',0.5)
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.set_eff('inner_ankle',0)
            self.JC.send_command()

        if which == "outer":
            self.JC.set_eff('left_ankle',-3)
            self.JC.set_eff('right_ankle',-3)
            self.JC.set_eff('left_hip',-3.0)
            self.JC.set_eff('right_hip',-3.0)
            self.JC.set_pos('inner_ankle',0.1)
            self.JC.send_command()
            rospy.sleep(0.1)
            #JC.set_pos('inner_ankle',0.0)
            self.JC.set_eff('left_ankle',0.1)
            self.JC.set_eff('right_ankle',0.1)
            self.JC.send_command()
            rospy.sleep(0.55)
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.set_eff('inner_ankle',0.5)
            self.JC.send_command()
            rospy.sleep(0.1)
            self.JC.set_eff('left_ankle',0)
            self.JC.set_eff('right_ankle',0)
            self.JC.send_command()

    def TakeStep(self,which):
        TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

        if TimeElapsed<60:
            Aperture = abs(self.RS.GetJointPos('left_hip'))
            self.SwingEff = self.SwingEff0 + self.kAp*(self.DesAp - Aperture)
            #self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)
            
            if which == 'outer':
				Aperture = self.RS.GetJointPos('left_hip')
            else:
				Aperture = -self.RS.GetJointPos('left_hip')
            self.ToeOffEff = self.ToeOffEff0 - 1*Aperture
            #self.SupAnkSetPoint = -0.02 + 0.03 * Aperture
			
            print Aperture
        else:
            Aperture = abs(self.RS.GetJointPos('left_hip'))
            #self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)
            #print self.ToeOffEff
            #print self.SwingEff
        if which == "outer":
            # Toe off
            self.JC.set_gains('inner_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
            self.JC.set_pos('inner_ankle',self.SupAnkSetPoint)
            self.JC.set_eff('left_ankle',self.ToeOffEff)
            self.JC.set_eff('right_ankle',self.ToeOffEff)
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.send_command()
            rospy.sleep(self.ToeOffDur)
            # Raise feet and swing
            self.JC.set_eff('left_ankle',-3)
            self.JC.set_eff('right_ankle',-3)
            self.JC.set_eff('left_hip',-self.SwingEff)
            self.JC.set_eff('right_hip',-self.SwingEff)
            self.JC.send_command()
            rospy.sleep(self.TotSwingDur-self.FootExtDur-self.FreeSwingDur)
            # Lower feet
            self.JC.set_eff('left_ankle',self.FootExtEff)
            self.JC.set_eff('right_ankle',self.FootExtEff)
            self.JC.send_command()
            rospy.sleep(self.FootExtDur)
            # End swing
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.set_eff('left_ankle',0)
            self.JC.set_eff('right_ankle',0)
            self.JC.send_command()
            rospy.sleep(self.FreeSwingDur)

        if which == "inner":
            # Toe off
            self.JC.set_gains('left_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
            self.JC.set_gains('right_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
            self.JC.set_pos('left_ankle',self.SupAnkSetPoint)
            self.JC.set_pos('right_ankle',self.SupAnkSetPoint)
            self.JC.set_eff('inner_ankle',self.ToeOffEff)
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.send_command()
            rospy.sleep(self.ToeOffDur)
            # Raise feet and swing
            self.JC.set_eff('inner_ankle',-3)
            self.JC.set_eff('left_hip',self.SwingEff)
            self.JC.set_eff('right_hip',self.SwingEff)
            self.JC.send_command()
            rospy.sleep(self.TotSwingDur-self.FootExtDur-self.FreeSwingDur)
            # Lower feet
            self.JC.set_eff('inner_ankle',self.FootExtEff)
            self.JC.send_command()
            rospy.sleep(self.FootExtDur)
            # End swing
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.set_eff('inner_ankle',0)
            self.JC.send_command()
            rospy.sleep(self.FreeSwingDur)

    def ResetPose(self):
        self.JC.set_pos('left_hip',0)
        self.JC.set_pos('left_ankle',0)
        self.JC.set_pos('right_hip',0)
        self.JC.set_pos('right_ankle',0)
        self.JC.set_pos('inner_ankle',0)
        self.JC.send_command()

    def RS_cb(self,msg):
        self.RS.UpdateState(msg)

    def Odom_cb(self,msg):
        self.GlobalPos = msg.pose.pose.position

    def reset(self):
        self.reset_srv()
        rospy.sleep(0.5)

        while self.GlobalPos.z<0.95 or self.GlobalPos.z>1.05 or abs(self.GlobalPos.x)>0.5:
            self.reset_srv()
            rospy.sleep(0.5)

    def Run(self,TimeOut = 0):
        rospy.sleep(0.1)

        pos = zeros(6)
        pos[0] = 1
        self.JC.reset_command()
        self.JC.send_pos_traj(zeros(6),pos,0.5,0.01) 
class GCB_Controller(object):
    """Ghost CB_Controller for the compas biped model that uses 2 overlapping legs"""

    def __init__(self, arg):
        super(GCB_Controller, self).__init__()

        ##################################################################
        ######################## GAIT PARAMETERS #########################
        ##################################################################
        if len(arg) < 12:
            self.SwingEff0 = 0.45  # Default swing effort
            self.kAp = 2  # Aperture feedback gain
            self.DesAp = 0.35  # Desired aperture
            self.SupAnkGain_p = 10  # Ankle gain p during support
            self.SupAnkGain_d = 5  # Ankle gain d during support
            self.SupAnkSetPoint = -0.03  # Support ankle set point
            self.ToeOffEff0 = 4.0  # Toe-off effort
            self.ToeOffDur = 0.16  # Toe-off duration
            self.TotSwingDur = 0.85  # Swing duration
            self.FootExtEff = 0.85  # Foot extension effort (right before touch down)
            self.FootExtDur = 0.18  # Foot extension duration
            self.FreeSwingDur = 0.15  # Swing duration

            self.SwingEff = self.SwingEff0
            self.ToeOffEff = self.ToeOffEff0
        else:
            self.SwingEff0 = arg[0]  # Default swing effort
            self.kAp = arg[1]  # Aperture feedback gain
            self.DesAp = arg[2]  # Desired aperture
            self.SupAnkGain_p = arg[3]  # Ankle gain p during support
            self.SupAnkGain_d = arg[4]  # Ankle gain d during support
            self.SupAnkSetPoint = arg[5]  # Support ankle set point
            self.ToeOffEff0 = arg[6]  # Toe-off effort
            self.ToeOffDur = arg[7]  # Toe-off duration
            self.TotSwingDur = arg[8]  # Swing duration
            self.FootExtEff = arg[9]  # Foot extension effort (right before touch down)
            self.FootExtDur = arg[10]  # Foot extension duration
            self.FreeSwingDur = arg[11]  # Swing duration

            self.SwingEff = self.SwingEff0
            self.ToeOffEff = self.ToeOffEff0

        self.StepsTaken = 0
        self.ApertureMean = 0
        self.ApertureStd = 0
        self.GlobalPos = 0

        self.reset_srv = rospy.ServiceProxy("/gazebo/reset_models", Empty)

        ##################################################################
        ###################### RAISE FOOT SEQUENCE #######################
        ##################################################################

        self.pos1 = zeros(3)

        # Sequence 1 for raising the left foot (from home position)
        self.seq1_1 = copy(self.pos1)
        self.seq1_1[0] = self.seq1_1[1] = 0.2

        self.seq1_2 = copy(self.seq1_1)
        self.seq1_2[1] = -1.45
        self.seq1_2[0] = 0.16
        self.seq1_2[2] = 0.06

        self.seq1_3 = copy(self.seq1_2)
        self.seq1_3[0] = -0.1

        # Sequence 2 for raising the right foot (from home position)
        self.seq2_1 = copy(self.pos1)
        self.seq2_1[0] = -0.2
        self.seq2_1[2] = 0.2

        self.seq2_2 = copy(self.seq2_1)
        self.seq2_2[2] = -1.45
        self.seq2_2[0] = -0.16
        self.seq2_2[1] = 0.06

        self.seq2_3 = copy(self.seq2_2)
        self.seq2_3[0] = 0.1

        ##################################################################
        ########################## INITIALIZE ############################
        ##################################################################

        # Initialize joint commands handler
        self._robot_name = "ghost_compass_biped"
        self._jnt_names = ["hip", "left_ankle", "right_ankle"]
        self.JC = JointCommands_msg_handler(self._robot_name, self._jnt_names)

        # Initialize robot state listener
        self.RS = robot_state(self._jnt_names)
        self.MsgSub = rospy.Subscriber(
            "/" + self._robot_name + "ghost_compass_biped/robot_state", RobotState, self.RS_cb
        )
        self.OdomSub = rospy.Subscriber("/ground_truth_odom", Odometry, self.Odom_cb)

    ##################################################################
    ########################### FUNCTIONS ############################
    ##################################################################

    def RaiseLeg(self, which):
        if which == "left":
            self.JC.send_pos_traj(self.pos1, self.seq1_1, 1, 0.01)
            rospy.sleep(0.2)
            self.JC.send_pos_traj(self.seq1_1, self.seq1_2, 1, 0.01)
            self.JC.set_eff("left_ankle", -5)
            self.JC.send_pos_traj(self.seq1_2, self.seq1_3, 1, 0.01)
            rospy.sleep(0.2)
        if which == "right":
            self.JC.send_pos_traj(self.pos1, self.seq2_1, 1, 0.01)
            rospy.sleep(0.2)
            self.JC.send_pos_traj(self.seq2_1, self.seq2_2, 1, 0.01)
            self.JC.set_eff("right_ankle", -5)
            self.JC.send_pos_traj(self.seq2_2, self.seq2_3, 1, 0.01)
            rospy.sleep(0.2)

    def TakeFirstStep(self, which):
        if which == "right":
            self.JC.set_eff("right_ankle", -3)
            self.JC.set_eff("hip", 5.5)
            self.JC.set_eff("left_ankle", 0.2)
            self.JC.send_command()
            rospy.sleep(0.1)
            self.JC.set_eff("right_ankle", 0)
            self.JC.send_command()
            rospy.sleep(0.55)
            self.JC.set_eff("hip", 0)
            self.JC.set_eff("left_ankle", 1)
            self.JC.send_command()
            rospy.sleep(0.1)

        if which == "left":
            self.JC.set_eff("left_ankle", -3)
            self.JC.set_eff("hip", -5.5)
            self.JC.set_pos("right_ankle", 0.2)
            self.JC.send_command()
            rospy.sleep(0.1)
            self.JC.set_eff("left_ankle", 0)
            self.JC.send_command()
            rospy.sleep(0.55)
            self.JC.set_eff("hip", 0)
            self.JC.set_eff("right_ankle", 1)
            self.JC.send_command()
            rospy.sleep(0.1)

    def TakeStep(self, which):
        TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

        if TimeElapsed < 60:
            Aperture = abs(self.RS.GetJointPos("hip"))
            self.SwingEff = self.SwingEff0 + self.kAp * (self.DesAp - Aperture)

            print Aperture
        else:
            Aperture = abs(self.RS.GetJointPos("hip"))
            # self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)
            # print self.ToeOffEff
            # print self.SwingEff
        if which == "right":
            # Toe off
            self.JC.set_gains("left_ankle", self.SupAnkGain_p, self.SupAnkGain_d, 0, set_default=False)
            self.JC.set_pos("left_ankle", self.SupAnkSetPoint)
            self.JC.set_eff("right_ankle", self.ToeOffEff)
            self.JC.set_eff("hip", 0)
            self.JC.send_command()
            rospy.sleep(self.ToeOffDur)
            # Raise feet and swing
            self.JC.set_eff("right_ankle", -3)
            self.JC.set_eff("hip", self.SwingEff)
            self.JC.send_command()
            rospy.sleep(self.TotSwingDur - self.FootExtDur - self.FreeSwingDur)
            # Lower feet
            self.JC.set_eff("right_ankle", self.FootExtEff)
            self.JC.send_command()
            rospy.sleep(self.FootExtDur)
            # End swing
            self.JC.set_eff("hip", 0)
            self.JC.set_eff("right_ankle", 0)
            self.JC.send_command()
            rospy.sleep(self.FreeSwingDur)

        if which == "left":
            # Toe off
            self.JC.set_gains("right_ankle", self.SupAnkGain_p, self.SupAnkGain_d, 0, set_default=False)
            self.JC.set_pos("right_ankle", self.SupAnkSetPoint)
            self.JC.set_eff("left_ankle", self.ToeOffEff)
            self.JC.set_eff("hip", 0)
            self.JC.send_command()
            rospy.sleep(self.ToeOffDur)
            # Raise feet and swing
            self.JC.set_eff("left_ankle", -3)
            self.JC.set_eff("hip", -self.SwingEff)
            self.JC.send_command()
            rospy.sleep(self.TotSwingDur - self.FootExtDur - self.FreeSwingDur)
            # Lower feet
            self.JC.set_eff("left_ankle", self.FootExtEff)
            self.JC.send_command()
            rospy.sleep(self.FootExtDur)
            # End swing
            self.JC.set_eff("hip", 0)
            self.JC.set_eff("left_ankle", 0)
            self.JC.send_command()
            rospy.sleep(self.FreeSwingDur)

    def ResetPose(self):
        self.JC.set_pos("hip", 0)
        self.JC.set_pos("left_ankle", 0)
        self.JC.set_pos("right_ankle", 0)
        self.JC.send_command()

    def RS_cb(self, msg):
        self.RS.UpdateState(msg)

    def Odom_cb(self, msg):
        self.GlobalPos = msg.pose.pose.position

    def reset(self):
        self.reset_srv()
        rospy.sleep(0.5)

        while self.GlobalPos.z < 0.95 or self.GlobalPos.z > 1.05 or abs(self.GlobalPos.x) > 0.5:
            self.reset_srv()
            rospy.sleep(0.5)

    def Run(self, TimeOut=0):
        rospy.sleep(0.1)

        # Start with right foot raised
        self.JC.send_pos_traj(self.RS.GetJointPos(), self.seq2_3, 0.5, 0.01)
        self.reset()
        rospy.sleep(1)

        self.StartTime = rospy.Time.now().to_sec()

        self.TakeFirstStep("right")
        self.ApertureMean = abs(self.RS.GetJointPos("hip"))

        self.Leg = "left"
        self.Go = 1
        while self.Go == 1:
            TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

            if self.GlobalPos.z < 0.6 or self.GlobalPos.z > 1.4 or (TimeElapsed > TimeOut and TimeOut > 0):
                # if the robot fell, stop running and return fitness
                return self.GlobalPos, self.ApertureStd

            self.TakeStep(self.Leg)

            self.StepsTaken += 1
            ThisAperture = abs(self.RS.GetJointPos("hip"))
            self.ApertureMean += (ThisAperture - self.ApertureMean) / self.StepsTaken
            self.ApertureStd += ((ThisAperture - self.ApertureMean) ** 2 - self.ApertureStd) / self.StepsTaken

            if self.Leg == "right":
                self.Leg = "left"
            else:
                self.Leg = "right"
Beispiel #6
0
class DW_Controller(object):
    """DW_Controller"""
    def __init__(self, iTf):
        super(DW_Controller, self).__init__()
        self._iTf = iTf

        ##################################################################
        ######################## GAIT PARAMETERS #########################
        ##################################################################
        self.CurSeqStep = 0
        self.CurSeqStep2 = 0
        self.Throtle = 1
        self.RotFlag = 0
        self.FollowPath = 0
        self.DesOri = 0

        self.count_tottal = 0
        self.count_tipping = 0

        self.BaseHipZ = 0.3

        ##################################################################
        ###################### Basic Standing Pose #######################
        ##################################################################

        self.BasStndPose = zeros(28)
        self.BasStndPose[5] = 0.1
        self.BasStndPose[5 + 6] = -0.1
        self.BasStndPose[6] = self.BasStndPose[6 + 6] = -0.2
        self.BasStndPose[7] = self.BasStndPose[7 + 6] = 0.4
        self.BasStndPose[8] = self.BasStndPose[8 + 6] = -0.2
        self.BasStndPose[9] = -0.1
        self.BasStndPose[9 + 6] = 0.1
        self.BasStndPose[17] = -1.3
        self.BasStndPose[17 + 6] = 1.3
        self.BasStndPose[18] = self.BasStndPose[18 + 6] = 1.5
        #         # Pose[18] -= 1.5
        #         # Pose[20] += 3.1
        # # self.BasStndPose[18] = self.BasStndPose[18+6] = 0
        # self.BasStndPose[20] = self.BasStndPose[20+6] = 1.5
        self.BasStndPose[21] = 0.4
        self.BasStndPose[27] = -0.4

        self.BaseHandPose = zeros(12)
        self.BaseHandPose[1] = self.BaseHandPose[1 + 3] = self.BaseHandPose[
            1 + 6] = 1.5

        ##################################################################
        ####################### Sit Down Sequence ########################
        ##################################################################

        self.SitDwnSeq1 = copy(self.BasStndPose)
        self.SitDwnSeq1[1] = 0.5
        self.SitDwnSeq1[4] = self.BaseHipZ  #0.1 ######### NEW #########
        self.SitDwnSeq1[4 + 6] = -self.BaseHipZ  #-0.1 ######### NEW #########
        self.SitDwnSeq1[5] = self.SitDwnSeq1[5 +
                                             6] = 0  ######### NEW #########
        self.SitDwnSeq1[6] = self.SitDwnSeq1[6 + 6] = -1.5
        self.SitDwnSeq1[7] = self.SitDwnSeq1[7 + 6] = 2.4
        self.SitDwnSeq1[8] = self.SitDwnSeq1[8 + 6] = -1.2
        self.SitDwnSeq1[16] = self.SitDwnSeq1[16 + 6] = 1.05
        self.SitDwnSeq1[17] = -1.25
        self.SitDwnSeq1[17 + 6] = 1.25
        self.SitDwnSeq1[18] = self.SitDwnSeq1[18 + 6] = 2.2
        self.SitDwnSeq1[21] = 0.2
        self.SitDwnSeq1[21 + 6] = -0.2

        ##################################################################
        ################# Crab Forward Walking Sequence ##################
        ##################################################################

        T = 1.5
        self.RobotCnfg = []
        self.StepDur = []

        # Sequence Step 1: Touch ground with pelvis, lift legs
        ThisRobotCnfg = copy(self.SitDwnSeq1)
        ThisRobotCnfg[1] = 0.9
        ThisRobotCnfg[8] = ThisRobotCnfg[8 + 6] = -0.1
        ThisRobotCnfg[6] = ThisRobotCnfg[6 + 6] = -1.9
        self.RobotCnfg.append(ThisRobotCnfg)
        self.StepDur.append(0.1 * T)

        # Sequence Step 2: Extend legs
        ThisRobotCnfg = copy(self.RobotCnfg[0][:])
        ThisRobotCnfg[7] = ThisRobotCnfg[7 + 6] = 1.1
        ThisRobotCnfg[8] = ThisRobotCnfg[8 + 6] = 0.8
        self.RobotCnfg.append(ThisRobotCnfg)
        self.StepDur.append(0.2 * T)

        # Sequence Step 3: Put legs down, bringing torso forward and raising arms
        ThisRobotCnfg = copy(self.RobotCnfg[1][:])
        ThisRobotCnfg[6] = ThisRobotCnfg[6 + 6] = -1.4
        ThisRobotCnfg[16] = ThisRobotCnfg[16 + 6] = 1.1
        ThisRobotCnfg[17] = -1.
        ThisRobotCnfg[17 + 6] = 1.
        ThisRobotCnfg[18] = ThisRobotCnfg[18 + 6] = 2.3
        ThisRobotCnfg[19] = 2
        ThisRobotCnfg[19 + 6] = -2
        self.RobotCnfg.append(ThisRobotCnfg)
        self.StepDur.append(0.6 * T)

        # Sequence Step 4: Touch ground with arms closer to pelvis and lift pelvis
        ThisRobotCnfg = copy(self.RobotCnfg[2][:])
        ThisRobotCnfg[16] = ThisRobotCnfg[16 + 6] = 0.6
        ThisRobotCnfg[17] = -1.35
        ThisRobotCnfg[17 + 6] = 1.35
        ThisRobotCnfg[18] = ThisRobotCnfg[18 + 6] = 2
        ThisRobotCnfg[19] = 0.1
        ThisRobotCnfg[19 + 6] = -0.1
        self.RobotCnfg.append(ThisRobotCnfg)
        self.StepDur.append(0.4 * T)

        # Sequence Step 5: Bring pelvis forward, closer to legs
        ThisRobotCnfg = copy(self.RobotCnfg[3][:])
        ThisRobotCnfg[7] = ThisRobotCnfg[7 + 6] = 2.4
        ThisRobotCnfg[8] = ThisRobotCnfg[8 + 6] = -0.2
        ThisRobotCnfg[16] = ThisRobotCnfg[16 + 6] = 1.0
        ThisRobotCnfg[17] = -1.1
        ThisRobotCnfg[17 + 6] = 1.1
        ThisRobotCnfg[18] = ThisRobotCnfg[18 + 6] = 2.5
        self.RobotCnfg.append(ThisRobotCnfg)
        self.StepDur.append(0.6 * T)

        ##################################################################
        ################# Crab Backward Walking Sequence #################
        ##################################################################

        T = 1
        self.RobotCnfg2 = []
        self.StepDur2 = []

        # Sequence Step 1: Bring pelvis down to the ground and lift arms
        ThisRobotCnfg = copy(self.SitDwnSeq1)
        ThisRobotCnfg[1] = 0.5
        ThisRobotCnfg[4] = self.BaseHipZ
        ThisRobotCnfg[4 + 6] = -self.BaseHipZ
        ThisRobotCnfg[6] = ThisRobotCnfg[6 + 6] = -1.7
        ThisRobotCnfg[7] = ThisRobotCnfg[7 + 6] = 1.0
        ThisRobotCnfg[8] = ThisRobotCnfg[8 + 6] = 0.8
        ThisRobotCnfg[16] = ThisRobotCnfg[16 + 6] = 1.5
        ThisRobotCnfg[17] = -0.2  #-0.6
        ThisRobotCnfg[17 + 6] = 0.2  #0.6
        ThisRobotCnfg[18] = ThisRobotCnfg[18 + 6] = 2.5
        ThisRobotCnfg[19] = 1.8
        ThisRobotCnfg[19 + 6] = -1.8
        ThisRobotCnfg[21] = -0.5
        ThisRobotCnfg[21 + 6] = 0.5
        self.RobotCnfg2.append(ThisRobotCnfg)
        self.StepDur2.append(0.3 * T)

        # Sequence Step 2: Extend arms
        ThisRobotCnfg = copy(self.RobotCnfg2[0][:])
        ThisRobotCnfg[16] = ThisRobotCnfg[16 + 6] = 1.2  #1.4
        ThisRobotCnfg[17] = -0.4  #-0.4#-0.2#-0.6#-0.4
        ThisRobotCnfg[17 + 6] = 0.4  #0.4#0.2#0.6# 0.4
        ThisRobotCnfg[18] = ThisRobotCnfg[18 + 6] = 2.9
        ThisRobotCnfg[19] = 0.2
        ThisRobotCnfg[19 + 6] = -0.2
        ThisRobotCnfg[21] = 0
        ThisRobotCnfg[21 + 6] = 0
        self.RobotCnfg2.append(ThisRobotCnfg)
        self.StepDur2.append(0.3 * T)

        # Sequence Step 3: Extend torso, fall back on arms, lift and fold legs
        ThisRobotCnfg = copy(self.RobotCnfg2[1][:])
        ThisRobotCnfg[1] = 0.6
        ThisRobotCnfg[4] = ThisRobotCnfg[4 + 6] = 0
        ThisRobotCnfg[5] = self.BaseHipZ
        ThisRobotCnfg[5 + 6] = -self.BaseHipZ
        ThisRobotCnfg[6] = ThisRobotCnfg[6 + 6] = -1.8
        ThisRobotCnfg[7] = ThisRobotCnfg[7 + 6] = 2.6
        ThisRobotCnfg[8] = ThisRobotCnfg[8 + 6] = 0.2
        ThisRobotCnfg[17] = -0.6  #-0.4#-0.2#-0.6#-0.4
        ThisRobotCnfg[17 + 6] = 0.6  #0.4#0.2#0.6# 0.4
        self.RobotCnfg2.append(ThisRobotCnfg)
        self.StepDur2.append(0.4 * T)

        # Sequence Step 4: Place legs on ground and lift pelvis
        ThisRobotCnfg = copy(self.RobotCnfg2[2][:])
        ThisRobotCnfg[1] = 0.3
        ThisRobotCnfg[6] = ThisRobotCnfg[6 + 6] = -0.6
        ThisRobotCnfg[7] = ThisRobotCnfg[7 + 6] = 2.0
        ThisRobotCnfg[8] = ThisRobotCnfg[8 + 6] = 0
        ThisRobotCnfg[18] = ThisRobotCnfg[18 + 6] = 2.8
        ThisRobotCnfg[19] = 0.4
        ThisRobotCnfg[19 + 6] = -0.4
        self.RobotCnfg2.append(ThisRobotCnfg)
        self.StepDur2.append(0.4 * T)

        # Sequence Step 5: Move pelvis back, between arms
        ThisRobotCnfg = copy(self.RobotCnfg2[3][:])
        ThisRobotCnfg[1] = 0.5
        ThisRobotCnfg[4] = self.BaseHipZ
        ThisRobotCnfg[4 + 6] = -self.BaseHipZ
        ThisRobotCnfg[5] = ThisRobotCnfg[5 + 6] = 0
        ThisRobotCnfg[6] = ThisRobotCnfg[6 + 6] = -1.7
        ThisRobotCnfg[7] = ThisRobotCnfg[7 + 6] = 1.0
        ThisRobotCnfg[8] = ThisRobotCnfg[8 + 6] = 0.8
        ThisRobotCnfg[16] = ThisRobotCnfg[16 + 6] = 0.5
        ThisRobotCnfg[17] = -1.35
        ThisRobotCnfg[17 + 6] = 1.35
        ThisRobotCnfg[18] = ThisRobotCnfg[18 + 6] = 2.3
        ThisRobotCnfg[19] = 0.5
        ThisRobotCnfg[19 + 6] = -0.5
        self.RobotCnfg2.append(ThisRobotCnfg)
        self.StepDur2.append(1 * T)  #was 0.7*T

        ##################################################################
        ########################## INITIALIZE ############################
        ##################################################################

    def Initialize(self, Terrain):
        self._robot_name = "atlas"
        self._jnt_names = [
            'back_lbz',
            'back_mby',
            'back_ubx',
            'neck_ay',  #3
            'l_leg_uhz',
            'l_leg_mhx',
            'l_leg_lhy',
            'l_leg_kny',
            'l_leg_uay',
            'l_leg_lax',  #9
            'r_leg_uhz',
            'r_leg_mhx',
            'r_leg_lhy',
            'r_leg_kny',
            'r_leg_uay',
            'r_leg_lax',  #15
            'l_arm_usy',
            'l_arm_shx',
            'l_arm_ely',
            'l_arm_elx',
            'l_arm_uwy',
            'l_arm_mwx',  #21
            'r_arm_usy',
            'r_arm_shx',
            'r_arm_ely',
            'r_arm_elx',
            'r_arm_uwy',
            'r_arm_mwx'
        ]  #27

        # Initialize joint commands handler
        self.JC = JointCommands_msg_handler(self._robot_name, self._jnt_names)
        self.LHC = hand_joint_controller("left")
        self.RHC = hand_joint_controller("right")
        # Initialize robot state listener
        self.RS = robot_state(self._jnt_names)
        self.IMU_mon = IMUCh()

        print("DW::Initialize")
        self.GlobalPos = 0
        self.GlobalOri = 0
        self._counter = 0
        self._terrain = Terrain
        self._fall_count = 0
        self.FALL_LIMIT = 3
        # self.reset_srv = rospy.ServiceProxy('/gazebo/reset_models', Empty)

        ##################################################################
        ######################## Controller Gains ########################
        ##################################################################

        self.JC.set_gains('l_leg_uhz', 1000, 0, 10)
        self.JC.set_gains('r_leg_uhz', 1000, 0, 10)
        self.JC.set_gains('l_leg_mhx', 1000, 0, 10)
        self.JC.set_gains('r_leg_mhx', 1000, 0, 10)
        self.JC.set_gains('back_lbz', 5000, 0, 10)
        self.JC.set_gains('back_ubx', 1000, 0, 10)
        self.JC.set_gains('l_arm_usy', 1000, 0, 10)
        self.JC.set_gains('r_arm_usy', 1000, 0, 10)
        self.JC.set_gains('l_arm_shx', 1000, 0, 10)
        self.JC.set_gains('r_arm_shx', 1000, 0, 10)
        self.JC.set_gains('l_arm_ely', 2000, 0, 10)
        self.JC.set_gains('r_arm_ely', 2000, 0, 10)
        self.JC.set_gains("l_arm_elx", 1200, 0, 5)
        self.JC.set_gains("r_arm_elx", 1200, 0, 5)
        self.JC.set_gains("l_arm_mwx", 1200, 0, 5)
        self.JC.set_gains("r_arm_mwx", 1200, 0, 5)

    ##################################################################
    ########################### FUNCTIONS ############################
    ##################################################################

    def RS_cb(self, msg):
        self.RS.UpdateState(msg)
        self.IMU_mon.get_contact(msg)
        self.IMU_mon.imu_manipulate(msg)

    def Odom_cb(self, msg):
        if 1000 <= self._counter:
            # print ("Odom_cb::", self.GlobalPos)
            self._counter = 0
        self._counter += 1
        self.GlobalPos = msg.pose.pose.pose.position  # from C25_GlobalPosition
        self.GlobalOri = msg.pose.pose.pose.orientation  # from C25_GlobalPosition
        # self.GlobalPos = msg.pose.pose.position    # from /ground_truth_odom
        # self.GlobalOri = msg.pose.pose.orientation # from /ground_truth_odom

    def move_neck(self):
        init_pos = self.RS.GetJointPos()
        self.JC.set_all_pos(init_pos)
        pos1 = self.RS.GetJointPos()[3]
        pos2 = 0.2  # 0.0 # 0.4 # Fire hose 0.7 #
        dt = 0.05
        N = 50
        for ratio in linspace(0, 1, N):
            interpCommand = (1 - ratio) * pos1 + ratio * pos2
            self.JC.set_pos(3, interpCommand)
            self.JC.send_command()
            rospy.sleep(dt)

    def ResetPose(self):
        self.JC.set_all_pos([0] * 28)
        self.JC.send_command()

    def reset(self):
        self.reset_srv()
        rospy.sleep(1)

        while self.GlobalPos.z < 0.9 or self.GlobalPos.z > 1:  #or abs(self.GlobalPos.x)>0.5:
            # while self.GlobalPos.z<0.25 or self.GlobalPos.z>0.4: #or abs(self.GlobalPos.x)>0.5:
            self.reset_srv()
            rospy.sleep(1)

    def current_ypr(self):

        quat = copy(self.GlobalOri)
        (r, p, y) = tf.transformations.euler_from_quaternion(
            [quat.x, quat.y, quat.z, quat.w])
        return (y, p, r)
        # RPY = self.RS.GetIMU()
        # return (RPY[2], RPY[1], RPY[0])

    def DeltaAngle(self, DesAngle, CurAngle):
        Delta = DesAngle - CurAngle
        if Delta > math.pi:
            Delta -= 2 * math.pi
        elif Delta < -math.pi:
            Delta += 2 * math.pi
        return Delta

    def SeqWithBalance(self, pos1, pos2, T, dt, COMref=0):
        if len(pos1) == len(pos2) == len(self._jnt_names):
            N = ceil(T / dt)
            pos1 = array(pos1)
            pos2 = array(pos2)

            init_com, rot = self._iTf.TransformListener().lookupTransform(
                '/com', '/l_foot', rospy.Time(0))
            if COMref != 0:
                COMref0 = [init_com[0], init_com[1]]
                init_com = [0] * 2

            for ratio in linspace(0, 1, N):
                cur_com, rot = self._iTf.TransformListener().lookupTransform(
                    '/com', '/l_foot', rospy.Time(0))
                interpCommand = (1 - ratio) * pos1 + ratio * pos2

                if COMref != 0:
                    init_com[0] = (1 - ratio) * COMref0[0] + ratio * COMref[0]
                    init_com[1] = (1 - ratio) * COMref0[1] + ratio * COMref[1]

                # Balance torso
                back_mby_pos = interpCommand[1] + 3 * (cur_com[0] -
                                                       init_com[0])
                back_ubx_pos = interpCommand[2] + 2 * (cur_com[1] -
                                                       init_com[1])

                self.JC.set_all_pos([float(x) for x in interpCommand])
                self.JC.set_pos("back_mby", back_mby_pos)
                self.JC.set_pos("back_ubx", back_ubx_pos)

                self.JC.send_command()
                rospy.sleep(dt)

        else:
            print 'position command legth doest fit'

    # def TimeVariantIterp(self,pos1,pos2,T_nom,dt_nom,vel_ind):
    #     if len(pos1) == len(pos2) == len(self._jnt_names):
    #         N = ceil(T_nom/dt_nom)
    #         pos1 = array(pos1)
    #         pos2 = array(pos2)
    #         K =100
    #         for ratio in linspace(0, 1, N):
    #             interpCommand = (1-ratio)*pos1 + ratio * pos2
    #             if np.linalg.norm(interpCommand - pos2) <= 0.01:
    #                 return
    #             self.JC.set_all_pos([ float(x) for x in interpCommand ])
    #             self.JC.send_command()
    #             vel = self.RS.GetAngVel()
    #             dt = max(dt_nom/(K*vel[vel_ind]),dt_nom/100)
    #             rospy.sleep(dt)

    #     else:
    #         print 'position command legth doest fit'

    def Sit(self, T):
        self.JC.set_gains("l_arm_elx", 5, 0, 20)
        self.JC.set_gains("r_arm_elx", 5, 0, 20)
        self.SeqWithBalance(self.RS.GetJointPos(), self.SitDwnSeq1, T * 0.3,
                            0.005)
        self.JC.set_pos("l_leg_uay", -0.1)
        self.JC.set_pos("r_leg_uay", -0.1)
        self.JC.set_gains("l_leg_uay", 10, 0, 5, set_default=False)
        self.JC.set_gains("r_leg_uay", 10, 0, 5, set_default=False)
        self.JC.set_gains("l_leg_lax", 10, 0, 5, set_default=False)
        self.JC.set_gains("r_leg_lax", 10, 0, 5, set_default=False)
        self.JC.send_command()
        rospy.sleep(T * 0.2)
        self.JC.set_gains("l_arm_elx", 1200, 0, 5)
        self.JC.set_gains("r_arm_elx", 1200, 0, 5)
        self.JC.set_gains("l_leg_uay", 50, 0, 5, set_default=False)
        self.JC.set_gains("r_leg_uay", 50, 0, 5, set_default=False)
        self.JC.set_gains("l_leg_lax", 50, 0, 5, set_default=False)
        self.JC.set_gains("r_leg_lax", 50, 0, 5, set_default=False)
        self.JC.send_command()
        rospy.sleep(T * 0.4)

    def GoToPoint(self, Point):
        if Point[2] == "fwd":
            self.Throtle = 1
        if Point[2] == "bwd":
            self.Throtle = 0.6
        self._Point = Point
        # Calculate distance and orientation to target
        while (0 == self.GlobalPos):
            rospy.sleep(1)
            print("Waiting for GlobalPos")
        DeltaPos = [
            self._Point[0] - self.GlobalPos.x,
            self._Point[1] - self.GlobalPos.y
        ]
        Distance = math.sqrt(DeltaPos[0]**2 + DeltaPos[1]**2)

        # Get current orientation
        y, p, r = self.current_ypr()
        T_ori = math.atan2(DeltaPos[1], DeltaPos[0])
        self.DesOri = T_ori
        if self._Point[2] == "bwd":
            T_ori += math.pi

        # Rotate in place towards target
        if abs(self.DeltaAngle(T_ori, y)) > 0.1:
            self.RotateToOri(T_ori)

        # Crawl towards target
        if self._Point[2] == "fwd":
            self.Crawl()
        if self._Point[2] == "bwd":
            self.BackCrawl()

        self._stuck_counter = 0

    def PerformStep(self):
        result = False
        self.count_tottal += 1
        print 'Total_count:', self.count_tottal
        # Calculate distance and orientation to target
        DeltaPos = [
            self._Point[0] - self.GlobalPos.x,
            self._Point[1] - self.GlobalPos.y
        ]
        Distance = math.sqrt(DeltaPos[0]**2 + DeltaPos[1]**2)
        T_ori = math.atan2(DeltaPos[1], DeltaPos[0])
        self.DesOri = T_ori
        if self._Point[2] == "bwd":
            T_ori += math.pi

        if Distance < 0.8:
            print "Reached Waypoint"
            result = True
        else:
            y, p, r = self.current_ypr()
            # Check tipping
            self.CheckTipping()

            # Rotate in place towards target
            if self._fall_count < self.FALL_LIMIT:
                # Crawl towards target
                if self._Point[2] == "fwd":
                    self.Crawl()
                if self._Point[2] == "bwd":
                    self.BackCrawl()
                Drift = abs(self.DeltaAngle(T_ori, y))
                if 0.5 < Drift < 1.4 and Distance > 1:
                    self.RotateToOri(T_ori)
                if Drift > 1.4:
                    self.RotateToOri(T_ori)

                    self.BackCrawl()
                DeltaPos2 = [
                    self._Point[0] - self.GlobalPos.x,
                    self._Point[1] - self.GlobalPos.y
                ]
                Distance2 = math.sqrt(DeltaPos2[0]**2 + DeltaPos2[1]**2)
                if self._terrain != 'MUD':
                    if abs(Distance2 - Distance) < 0.1:
                        self._stuck_counter += 1
                        print 'stuck... D=', (Distance2 - Distance)
                        if self._stuck_counter >= 2:
                            #dont follow path
                            self.FollowPath = 0
                            #go oposite dir
                            if self._Point[2] == "bwd":
                                self.Crawl()
                            if self._Point[2] == "fwd":
                                self.BackCrawl()
                            self.RotSpotSeq(2)
                            self.FollowPath = 1
                            self._stuck_counter = 0
                    else:
                        self._stuck_counter = 0

            else:
                self.FollowPath = 0
        return result

    def Crawl(self):
        # self.IMU_mon.turned = 0
        if self.FollowPath == 1:
            # Update sequence to correct orientation
            y, p, r = self.current_ypr()
            Correction = self.DeltaAngle(self.DesOri, y)

            Delta = Correction / 0.15
            if abs(Delta) > 1:
                Delta /= abs(Delta)

            self.AddRotation(Delta)

        self.GoToSeqStep(5)
        self.RotFlag = 0

    def BackCrawl(self):
        # self.IMU_mon.turned = 1
        if self.FollowPath == 1:
            # Update sequence to correct orientation
            y, p, r = self.current_ypr()
            Correction = self.DeltaAngle(self.DesOri + math.pi, y)

            Delta = Correction / 0.22
            if abs(Delta) > 1:
                Delta /= abs(Delta)

            self.AddBackRotation(Delta)

        #     if self.RotFlg == 1:
        #         self.AddBackRotation(Delta)

        if self.RotFlag == 1:
            self.RotFlag = 2
            self.GoToBackSeqStep(1)

        self.GoToBackSeqStep(5)

    def GoToSeqStep(self, Step):
        if Step == self.CurSeqStep:
            pass
        else:
            if Step > 4:
                Step = 0
                self.DoSeqStep()
            while self.CurSeqStep != Step:
                self.DoSeqStep()

    def GoToBackSeqStep(self, Step):
        if Step == self.CurSeqStep2:
            pass
        else:
            if Step > 4:
                Step = 0
                self.DoInvSeqStep()
            while self.CurSeqStep2 != Step:
                self.DoInvSeqStep()

    def DoSeqStep(self):
        print 'Doing Step seq #', self.CurSeqStep
        #self.traj_with_impedance(self.RS.GetJointPos(),self.RobotCnfg[self.CurSeqStep],self.StepDur[self.CurSeqStep]/self.Throtle,0.005)
        self.JC.send_pos_traj(self.RS.GetJointPos(),
                              self.RobotCnfg[self.CurSeqStep],
                              self.StepDur[self.CurSeqStep] / self.Throtle,
                              0.005)
        self.JC.reset_gains()
        self.CurSeqStep += 1
        if self.CurSeqStep > 4:
            self.CurSeqStep = 0

    def DoInvSeqStep(self):
        if self._terrain == 'HILLS':
            if self.CurSeqStep == 2:  ###### NEW ######
                self.JC.set_gains('r_arm_ely', 600, 0, 30, set_default=False)
                self.JC.set_gains('l_arm_ely', 600, 0, 30, set_default=False)

            if self.CurSeqStep2 == 3:

                if self.IMU_mon.second_contact == 'arm_r':
                    self.JC.set_gains('r_arm_ely',
                                      3000,
                                      0,
                                      10,
                                      set_default=False)
                    self.JC.set_gains('l_arm_ely',
                                      1000,
                                      0,
                                      10,
                                      set_default=False)

                elif self.IMU_mon.second_contact == 'arm_l':
                    self.JC.set_gains('l_arm_ely',
                                      3000,
                                      0,
                                      10,
                                      set_default=False)
                    self.JC.set_gains('r_arm_ely',
                                      1000,
                                      0,
                                      10,
                                      set_default=False)

                print 'second_contact:', self.IMU_mon.second_contact

            print 'Doing inv. Step seq #', self.CurSeqStep2

            if self.CurSeqStep2 == 1 or self.CurSeqStep2 == 2:
                pos = list(self.RS.GetJointPos())
                pos[2] = -1.3 * self.IMU_mon.roll
                self.JC.send_pos_traj(list(self.RS.GetJointPos()), pos, 0.2,
                                      0.01)
                self.RobotCnfg2[self.CurSeqStep2][2] = pos[
                    2]  ###### NEW ######

                # pos = copy(self.RobotCnfg2[1][:])
                # self.TimeVariantIterp(self.RS.GetJointPos(),self.RobotCnfg2[self.CurSeqStep2],self.StepDur2[self.CurSeqStep2]/self.Throtle,0.005,0)
                # self.CurSeqStep2 += 1
                # if self.CurSeqStep2 > 4:
                #     self.CurSeqStep2 = 0
                # return

        self.JC.send_pos_traj(self.RS.GetJointPos(),
                              self.RobotCnfg2[self.CurSeqStep2],
                              self.StepDur2[self.CurSeqStep2] / self.Throtle,
                              0.005)
        self.JC.reset_gains()
        self.CurSeqStep2 += 1
        if self.CurSeqStep2 > 4:
            self.CurSeqStep2 = 0
        # if self.CurSeqStep2 > 4:
        #     self.CurSeqStep2 = 0

    def AddRotation(self, Delta):
        # Delta of 1 gives approx. 0.15 radians turn left
        # Add gait changes to appropriate step
        self.RobotCnfg[1][4] = self.BaseHipZ + Delta * 0.2
        self.RobotCnfg[1][4 + 6] = -self.BaseHipZ + Delta * 0.2
        self.RobotCnfg[1][9] = self.RobotCnfg[1][9 + 6] = Delta * 0.12
        self.RobotCnfg[4][4] = self.BaseHipZ
        self.RobotCnfg[4][4 + 6] = -self.BaseHipZ
        self.RobotCnfg[4][9] = self.RobotCnfg[4][9 + 6] = 0

        # Insert those changes in following steps as well
        self.RobotCnfg[2][4] = self.BaseHipZ + Delta * 0.2
        self.RobotCnfg[2][4 + 6] = -self.BaseHipZ + Delta * 0.2
        self.RobotCnfg[2][9] = self.RobotCnfg[2][9 + 6] = Delta * 0.12
        self.RobotCnfg[3][4] = self.BaseHipZ + Delta * 0.2
        self.RobotCnfg[3][4 + 6] = -self.BaseHipZ + Delta * 0.2
        self.RobotCnfg[3][9] = self.RobotCnfg[3][9 + 6] = Delta * 0.12
        self.RobotCnfg[0][4] = self.BaseHipZ
        self.RobotCnfg[0][4 + 6] = -self.BaseHipZ
        self.RobotCnfg[0][9] = self.RobotCnfg[0][9 + 6] = 0

    def AddBackRotation(self, Delta):
        # Delta of 1 gives approx. 0.22 radians turn left
        if Delta > 0:
            dID = 0
        else:
            dID = 6

        # Add gait changes to appropriate step
        self.RobotCnfg2[1][0] = 0.3 * Delta
        self.RobotCnfg2[1][2] = 0.15 * Delta
        self.RobotCnfg2[3][0] = 0
        self.RobotCnfg2[3][2] = 0

        # Insert those changes in following steps as well
        self.RobotCnfg2[2][0] = 0.3 * Delta
        self.RobotCnfg2[2][2] = 0.15 * Delta
        self.RobotCnfg2[3][0] = 0.3 * Delta
        self.RobotCnfg2[3][2] = 0.15 * Delta
        self.RobotCnfg2[4][0] = 0
        self.RobotCnfg2[4][2] = 0
        self.RobotCnfg2[0][0] = 0
        self.RobotCnfg2[0][2] = 0

    def RotateToOri(self, Bearing):
        if self._terrain == "MUD" or self._terrain == "HILLS":
            return self.RotateToOriInMud(Bearing)
        else:
            if self.RotFlag == 2:
                self.GoToBackSeqStep(1)
            self.RotFlag = 1

            # Make sure Bearing is from -pi to +pi
            Bearing = Bearing % (2 * math.pi)
            if Bearing > math.pi:
                Bearing -= 2 * math.pi
            if Bearing < -math.pi:
                Bearing += 2 * math.pi

            # Get into "break-dance" configuration
            #self.GoToSeqStep(3)
            pos = copy(self.RobotCnfg[2][:])
            pos[1] = 0.9
            pos[6] = pos[6 + 6] = -1.7
            pos[8] = pos[8 + 6] = 0.7
            pos[16] = pos[16 + 6] = 0.9
            pos[17] = -0.9
            pos[17 + 6] = 0.9
            pos[18] = pos[18 + 6] = 2
            pos[19] = 1.0
            pos[19 + 6] = -1.0
            self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1, 0.01)

            # Get current orientation
            y0, p, r = self.current_ypr()
            Angle = self.DeltaAngle(Bearing, y0)

            while abs(Angle) > 0.3:  #0.15: # Error of 9 degrees
                print 'Delta: ', Angle, 'Bearing: ', Bearing, 'yaw: ', y0
                Delta = Angle / 0.75
                if abs(Delta) > 1:
                    Delta /= abs(Delta)
                if 0 < Delta < 0.35:
                    Delta += 0.25
                if -0.35 < Delta < 0:
                    Delta -= 0.25

                self.RotSpotSeq(Delta)

                # Check tipping
                self.CheckTipping()

                # Get current orientation
                y, p, r = self.current_ypr()
                if abs(self.DeltaAngle(y, y0)) < 0.1:
                    # Robot isn't turning, try RotateInMud
                    return self.RotateToOriInMud(Bearing)

                y0 = y
                Angle = self.DeltaAngle(Bearing, y0)

            # # Return to original configuration

            # self.JC.send_pos_traj(self.RS.GetJointPos(),self.RobotCnfg[0][:],0.5,0.01)
            # self.CurSeqStep = 0
            # self.CurSeqStep2 = 0
            #############################################
            # Return to original configuration

            self.JC.send_pos_traj(self.RS.GetJointPos(), self.RobotCnfg2[4][:],
                                  2, 0.01)
            rospy.sleep(1)
            self.CurSeqStep2 = 4
            return 1

    def RotateToOriInMud(self, Bearing):
        self.RotFlag = 1

        # Make sure Bearing is from -pi to +pi
        Bearing = Bearing % (2 * math.pi)
        if Bearing > math.pi:
            Bearing -= 2 * math.pi
        if Bearing < -math.pi:
            Bearing += 2 * math.pi

        self.JC.send_pos_traj(self.RS.GetJointPos(), self.RobotCnfg2[4][:],
                              1.5, 0.01)

        # Get current orientation
        y0, p, r = self.current_ypr()
        Angle = self.DeltaAngle(Bearing, y0)
        # print 'Angle: ',Angle
        while abs(Angle) > 0.1:  # Error of 3 degrees
            print 'MUD Delta: ', Angle, 'Bearing: ', Bearing, 'yaw: ', y0
            Delta = Angle / 0.45
            if abs(Delta) > 1:
                Delta /= abs(Delta)
            # if 0<Delta<0.35:
            #     Delta+=0.25
            # if -0.35<Delta<0:
            #     Delta-=0.25

            self.RotOnMudSeq(Delta)
            # Check tipping
            self.CheckTipping()
            # Get current orientation
            y, p, r = self.current_ypr()
            if abs(self.DeltaAngle(y, y0)) < 0.1:
                # Robot isn't turning, Give up
                return 0

            y0 = y
            # Angle=self.DeltaAngle(Bearing,y0)
            Angle = self.DeltaAngle(Bearing, y0)

        # # Return to original configuration
        # self.JC.send_pos_traj(self.RS.GetJointPos(),self.RobotCnfg[0][:],0.5,0.01)
        # self.CurSeqStep = 0
        # self.CurSeqStep2 = 0

        #############################################
        # Return to original configuration

        self.JC.send_pos_traj(self.RS.GetJointPos(), self.RobotCnfg2[4][:], 2,
                              0.01)
        rospy.sleep(1)
        self.CurSeqStep2 = 4
        return 1

    def RotSpotSeq(self, Delta):
        # Delta of 1 gives a left rotation of approx. 0.75 radians

        # Init configuration
        pos = copy(self.RobotCnfg[2][:])
        pos[1] = 0.8
        pos[6] = pos[6 + 6] = -1.7
        pos[8] = pos[8 + 6] = 0.8
        pos[16] = pos[16 + 6] = 0.9
        pos[17] = -0.9
        pos[17 + 6] = 0.9
        pos[18] = pos[18 + 6] = 2
        pos[19] = 1.0
        pos[19 + 6] = -1.0

        T = 1.
        if Delta > 0:
            sign = 1
            dID = 0
        else:
            sign = -1
            dID = 6
        Delta = abs(Delta)

        # Lift first leg
        pos[7 + dID] = 1.3 - 0.5 * Delta
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.05 * T, 0.005)
        # Rotate first leg outwards until it touches ground
        pos[4 + dID] = Delta * sign * 0.35
        pos[4 + 6 - dID] = -Delta * sign * 0.35
        pos[5 + 6 - dID] = -Delta * sign * 0.2
        pos[5 + dID] = Delta * sign * 0.2
        pos[9 + dID] = Delta * sign * 0.2
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.3 * T, 0.005)
        # Lift other leg, now all the weight is on first leg
        pos[7 + dID] = 1.3 - 0.1 * Delta
        pos[7 + 6 - dID] = 1.3 - 0.5 * Delta
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.05 * T, 0.005)
        # Return first leg and torso to original configuration
        pos[4 + dID] = 0
        pos[4 + 6 - dID] = 0
        pos[5 + 6 - dID] = 0
        pos[5 + dID] = 0
        pos[7 + dID] = 1.3
        pos[9 + dID] = 0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5 * T, 0.005)
        # Return other leg to original configuration
        pos[7 + 6 - dID] = 1.3
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.1 * T, 0.005)
        rospy.sleep(0.1 * T)

    def RotOnMudSeq(self, Delta):
        # Delta of 1 gives a left rotation of approx. 0.45 radians
        if Delta > 0:
            dID = 0
        else:
            dID = 6

        T = 1

        # Get into starting position
        pos = copy(self.RobotCnfg2[4][:])
        pos[1] = 0.8
        # Get current orientation
        y0, p, r = self.current_ypr()
        pos[2] = -0.5 * r
        pos[6] = pos[6 + 6] = -1.3
        pos[16] = pos[16 + 6] = 0.6
        pos[17] = -1.2
        pos[17 + 6] = 1.2
        pos[18] = pos[18 + 6] = 1.8
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5 * T, 0.01)
        # pos[6] -= 0.2

        # Lift first leg
        pos[2] = 0
        pos[6 + dID] = -1.7
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2 * T, 0.01)

        # Rotate it to the side
        pos[4 + dID] = 0.8 * Delta
        pos[0] = -0.4 * Delta
        pos[4 + 6 - dID] = -0.4 * Delta
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.4 * T, 0.01)

        # Lower first leg / Lift second leg
        pos[6 + dID] = -1.3
        pos[6 + 6 - dID] = -1.8
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2 * T, 0.01)

        # Rotate pelvis / Close first leg
        pos[0] = -0.8 * Delta
        pos[4 + dID] = 0
        pos[4 + 6 - dID] = 0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.4 * T, 0.01)

        # Return to init
        pos[1] = 0.9
        pos[6] = pos[6 + 6] = -1.3
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2 * T, 0.01)

        # Lift arms
        pos = copy(self.RobotCnfg2[0][:])
        y0, p, r = self.current_ypr()
        pos[2] = -0.5 * r
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.6 * T, 0.01)

    def CheckTipping(self):

        result = 0
        while result == 0:
            # Get current orientation

            y, p, r = self.current_ypr()
            self._fall_count += 1
            R, P, Y = self.RS.GetIMU()

            print 'Check Tipping r=', r, "R=", R, "p=", p, "P=", P
            # if abs(p)>0.4*math.pi or abs(r)>0.8*math.pi:
            if P >= 0.8:
                # print "Front recovery"
                result = self.FrontTipRecovery()
            elif abs(p) > 0.4 * math.pi or abs(r) > 0.8 * math.pi:
                # Robot tipped backwards
                # print "Back recovery"
                result = self.BackTipRecovery()
                self.CurSeqStep2 = 0  ########
                self.CurSeqStep = 0  ###############

            elif r > math.pi / 4:
                # Robot tipped to the right
                result = self.TipRecovery("right")
                self.CurSeqStep2 = 0  ########
                self.CurSeqStep = 0  ###############
                # print "Right recovery"
            elif r < -math.pi / 4:
                # Robot tipped to the left
                result = self.TipRecovery("left")
                self.CurSeqStep2 = 0  ########
                self.CurSeqStep = 0  ###############
                # print "Left recovery"
            else:
                result = 1
                self.FollowPath = 1
                self._fall_count = 0

    def TipRecovery(self, side):
        self.count_tipping += 1
        print 'Tipping:', self.count_tipping
        if side == "right":
            dID = 0
        sign = 1
        if side == "left":
            dID = 6
            sign = -1

        # Extend legs and flex arm
        pos = copy(self.RobotCnfg[4][:])
        pos[7] = pos[7 + 6] = 0.8
        pos[17 + 6 - dID] = 0
        #        pos[18+6-dID] = 2.8
        pos[19 + 6 - dID] = -sign * 1.8
        pos[21 + 6 - dID] = -sign * 1.4
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.4,0.01)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.01)

        # Widen leg stance, turn with hip
        pos[2] = -sign * 0.5
        pos[4] = 0.5
        pos[4 + 6] = -0.5
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.4,0.01)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.8, 0.01)
        ###############NEW #################

        R, P, Y = self.RS.GetIMU()

        if P >= 0.8:
            print "Front recovery"
            result = self.FrontTipRecovery()
            return result

        # Push with arm to rotate
        pos[16 + 6 - dID] = 0.2
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,1,0.01)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1.2, 0.01)

        rospy.sleep(0.5)

        # Return to final sequence pos (forward)
        pos = copy(self.RobotCnfg[4][:])
        pos[4] = 0.3
        pos[4 + 6] = -0.3
        pos[5] = 0.3
        pos[5 + 6] = -0.3
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.5,0.01)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.9, 0.01)
        rospy.sleep(1.5)

        # Get current orientation
        y, p, r = self.current_ypr()
        if abs(r) < math.pi / 4:
            # Success!
            self.JC.send_pos_traj(self.RS.GetJointPos(), self.RobotCnfg2[4][:],
                                  2, 0.01)
            rospy.sleep(1)
            self.CurSeqStep2 = 4
            # self.GoToBackSeqStep(5)
            return 1

        else:
            return 0

    def BackTipRecovery(self):
        # "Flatten" body on ground, bend elbows
        pos = copy(self.RobotCnfg[4][:])
        pos[1] = 0
        pos[6] = pos[6 + 6] = -0.4
        pos[7] = pos[7 + 6] = 0.8
        pos[8] = pos[8 + 6] = 0.3
        pos[16] = pos[16 + 6] = 0
        pos[17] = 0.4
        pos[17 + 6] = -0.4
        pos[18] = pos[18 + 6] = 3.14
        pos[19] = 1.1
        pos[19 + 6] = -1.1
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.5,0.01)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1, 0.01)

        # Raise torso on elbows
        pos[1] = 0.8
        pos[16] = pos[16 + 6] = 1.4
        pos[17] = 0
        pos[17 + 6] = 0
        pos[18] = pos[18 + 6] = 2.6
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.5,0.01)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1, 0.01)

        # Extend arms
        pos[1] = 0.8
        pos[6] = pos[6 + 6] = -1.4
        pos[7] = pos[7 + 6] = 2.2
        pos[16] = pos[16 + 6] = 1.4
        pos[17] = -1.3
        pos[17 + 6] = 1.3
        pos[19] = pos[19 + 6] = 0
        self.JC.send_pos_traj(
            self.RS.GetJointPos(), pos, 2.2,
            0.01)  #self.JC.send_pos_traj(self.RS.GetJointPos(),pos,2,0.01)

        rospy.sleep(1.5)

        # Get current orientation
        y, p, r = self.current_ypr()
        if abs(p) < 0.4 * math.pi:
            # Success!
            self.JC.send_pos_traj(self.RS.GetJointPos(), self.RobotCnfg2[4][:],
                                  2, 0.01)
            rospy.sleep(1)
            self.CurSeqStep2 = 4
            # self.GoToBackSeqStep(5)
            return 1
        else:
            return 0

    def FrontTipRecovery(self):

        pos = [
            0, 0, 0, 0, 0, 0, -0.02, 0.04, -0.02, 0, 0, 0, -0.02, 0.04, -0.02,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
        ]
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.5,0.01)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 2, 0.01)

        pos = [
            0, 0, 0, 0, 0, 0, 0, 0, -2.1, 0, 0, 0, 0, 0, -2.1, 0, 0, 0, 0, 1.5,
            0, 0, 0, 0, 0, -1.5, 0, 0
        ]
        # self.JC.send_pos_traj(self.RS.GetJointPos(),pos,0.5,0.01)
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1.5, 0.01)

        pos = [
            0, 0.8, 0, 0, 0, 0, -2, 2, -2.1, 0, 0, 0, -2, 2, -2.1, 0, -1.7,
            -0.3, 0, 1.5, 0, 0, -1.7, 0.3, 0, -1.5, 0, 0
        ]
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.01)

        pos = [
            0, 1.1, 0, 0, 0, 0, -2.2, 2.8, -2.6, 0, 0, 0, -2.2, 2.8, -2.6, 0,
            -1.4, -0.8, 0, 0, 0, 1.5, -1.4, 0.8, 0, 0, 0, -1.5
        ]
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.01)

        pos = [
            0, 0.8, 0, 0, 0, 0, -2, 2.8, -2.6, 0, 0, 0, -2, 2.8, -2.6, 0, -1.5,
            -0.8, 0, 0, 0, 0, -1.5, 0.8, 0, 0, 0, 0
        ]
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.2, 0.01)
        rospy.sleep(1)
        pos = copy(self.RobotCnfg[4][:])
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1, 0.01)
        # Get current orientation
        y, p, r = self.current_ypr()
        if abs(p) < 0.4 * math.pi:
            # Success!
            self.JC.send_pos_traj(self.RS.GetJointPos(), self.RobotCnfg2[4][:],
                                  2, 0.01)
            rospy.sleep(1)
            self.CurSeqStep2 = 4
            # self.GoToBackSeqStep(5)
            return 1
        else:
            return 0

    def DynStandUp(self):
        # Works only a small percentage of the time
        # SeqWithBalance needs to be updated to include a COM horizontal motion
        # (since the hip begins the motion from behind the feet)
        self.JC.set_gains("l_arm_mwx", 1200, 0, 5)
        self.JC.set_gains("r_arm_mwx", 1200, 0, 5)

        T = 1

        # Go to init pos, supported on hands and feet, with pelvis back between hands
        pos = copy(self.RobotCnfg2[4][:])
        pos[7] = pos[7 + 6] = 1.2
        pos[8] = pos[8 + 6] = 0.7
        pos[18] = pos[18 + 6] = 1.2
        pos[20] = pos[20 + 6] = 1
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 1.2 * T, 0.005)

        # [Insert description here]
        pos[16] = pos[16 + 6] = 0.3
        pos[17] = -1.4
        pos[17 + 6] = 1.4
        pos[19] = pos[19 + 6] = 0
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.6 * T, 0.005)

        rospy.sleep(0.5)

        # Tuck legs in and start rotation
        pos[1] = 0.8
        pos[5] = 0.1
        pos[5 + 6] = -0.1
        pos[6] = -1.7
        pos[6 + 6] = -1.7
        pos[7] = 2.4
        pos[7 + 6] = 2.4
        pos[9] = -0.1
        pos[9 + 6] = 0.1
        pos[16] = pos[16 + 6] = -0.5
        pos[17] = -1.2
        pos[17 + 6] = 1.2
        pos[8] = pos[8 + 6] = -0.7
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.5 * T, 0.005)

        pos[1] = 0.2
        pos[7] = 2.1
        pos[7 + 6] = 2.1
        pos[16] = pos[16 + 6] = -1.4
        pos[17] = -0.7
        pos[17 + 6] = 0.7
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.6 * T, 0.005)

        rospy.sleep(1)

        # Use hands to push off and place COM on feet
        self.JC.set_gains('l_leg_uay', 900, 0, 10)
        self.JC.set_gains('r_leg_uay', 900, 0, 10)
        self.JC.set_gains('l_leg_lax', 900, 0, 10)
        self.JC.set_gains('r_leg_lax', 900, 0, 10)
        self.JC.set_gains('l_arm_usy', 1000, 0, 10)
        self.JC.set_gains('r_arm_usy', 1000, 0, 10)
        self.JC.set_gains('l_arm_shx', 1000, 0, 10)
        self.JC.set_gains('r_arm_shx', 1000, 0, 10)
        pos[1] = 0.7
        pos[7] = 2.4
        pos[7 + 6] = 2.4
        pos[16] = pos[16 + 6] = -1.4
        pos[17] = -1.1
        pos[17 + 6] = 1.1
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.6 * T, 0.005)

        rospy.sleep(0.5)
        pos[17] = -0.9
        pos[17 + 6] = 0.9
        self.JC.send_pos_traj(self.RS.GetJointPos(), pos, 0.6 * T, 0.005)
        rospy.sleep(0.5)

        rospy.sleep(2)
        self.SeqWithBalance(self.RS.GetJointPos(), self.BasStndPose, 3, 0.005,
                            [-0.02, 0.175])
        rospy.sleep(0.8)
        self.JC.send_pos_traj(self.RS.GetJointPos(), self.BasStndPose, 0.5 * T,
                              0.005)

    def StandUp(self):
        RPY = self.RS.GetIMU()
        D, R = self._iTf.TransformListener().lookupTransform(
            '/l_foot', '/pelvis', rospy.Time(0))
        while not (abs(RPY[0]) <= 0.1 and abs(RPY[1]) <= 0.1 and D[2] >= 0.8):
            self.CheckTipping()
            self.DynStandUp()
            rospy.sleep(1)
            RPY = self.RS.GetIMU()
            D, R = self._iTf.TransformListener().lookupTransform(
                '/l_foot', '/pelvis', rospy.Time(0))
            print 'roll: ', RPY[0], 'pitch: ', RPY[1], 'D: ', D[2]
class CB_Controller(object):
    """CB_Controller"""
    def __init__(self, arg):
        # super(CB_Controller, self).__init__()

        ##################################################################
        ######################## GAIT PARAMETERS #########################
        ##################################################################
        if len(arg)<12:
            self.SwingEff0 = 0.46        # Default swing effort
            self.kAp = 2                 # Aperture feedback gain
            self.DesAp = 0.4             # Desired aperture
            self.SupAnkGain_p = 10       # Ankle gain p during support
            self.SupAnkGain_d = 4        # Ankle gain d during support
            self.SupAnkSetPoint = -0.02  # Support ankle set point
            self.ToeOffEff = 4           # Toe-off effort
            self.ToeOffDur = 0.12        # Toe-off duration
            self.TotSwingDur = 0.85      # Swing duration
            self.FootExtEff = 0.95       # Foot extension effort (right before touch down)
            self.FootExtDur = 0.18       # Foot extension duration
            self.FreeSwingDur = 0.15     # Swing duration
        else:
            self.SwingEff0 = arg[0]      # Default swing effort
            self.kAp = arg[1]            # Aperture feedback gain
            self.DesAp = arg[2]          # Desired aperture
            self.SupAnkGain_p = arg[3]   # Ankle gain p during support
            self.SupAnkGain_d = arg[4]   # Ankle gain d during support
            self.SupAnkSetPoint = arg[5] # Support ankle set point
            self.ToeOffEff = arg[6]      # Toe-off effort
            self.ToeOffDur = arg[7]      # Toe-off duration
            self.TotSwingDur = arg[8]    # Swing duration
            self.FootExtEff = arg[9]     # Foot extension effort (right before touch down)
            self.FootExtDur = arg[10]    # Foot extension duration
            self.FreeSwingDur = arg[11]  # Swing duration
        
        self.StepsTaken = 0
        self.ApertureMean = 0
        self.ApertureStd = 0
        self.GlobalPos = 0

        self.reset_srv = rospy.ServiceProxy('/gazebo/reset_models', Empty)

        ##################################################################
        ###################### RAISE FOOT SEQUENCE #######################
        ##################################################################

        self.pos1=zeros(5)

        # Sequence 1 for raising the inner foot (from home position)
        self.seq1_1=copy(self.pos1)
        self.seq1_1[1] = self.seq1_1[3] = self.seq1_1[0] = 0.2

        self.seq1_2=copy(self.seq1_1)
        self.seq1_2[0] = -1.45
        self.seq1_2[1] = self.seq1_2[3] = 0.16
        self.seq1_2[2] = self.seq1_2[4] = 0.06

        self.seq1_3=copy(self.seq1_2)
        self.seq1_3[1] = self.seq1_3[3] = 0.1

        # Sequence 2 for raising the outer feet (from home position)
        self.seq2_1=copy(self.pos1)
        self.seq2_1[1] = self.seq2_1[3] = -0.2
        self.seq2_1[2] = self.seq2_1[4] = 0.2

        self.seq2_2=copy(self.seq2_1)
        self.seq2_2[2] = self.seq2_2[4] = -1.45
        self.seq2_2[1] = self.seq2_2[3] = -0.16
        self.seq2_2[0] = 0.06

        self.seq2_3=copy(self.seq2_2)
        self.seq2_3[1] = self.seq2_3[3] = -0.1

        ##################################################################
        ########################## INITIALIZE ############################
        ##################################################################

        # Initialize joint commands handler
        self._robot_name = 'compass_biped'
        self._jnt_names = ['inner_ankle','left_hip','left_ankle','right_hip','right_ankle']
        self.JC = JointCommands_msg_handler(self._robot_name,self._jnt_names)

        # Initialize robot state listener
        self.RS = robot_state(self._jnt_names)
        self.MsgSub = rospy.Subscriber('/'+self._robot_name+'/robot_state',RobotState,self.RS_cb)
        self.OdomSub = rospy.Subscriber('/ground_truth_odom',Odometry,self.Odom_cb)


    ##################################################################
    ########################### FUNCTIONS ############################
    ##################################################################

    def RaiseLeg(self,which):
        if which == "inner":
            self.JC.send_pos_traj(self.pos1,self.seq1_1,1,0.01)
            rospy.sleep(0.2)
            self.JC.send_pos_traj(self.seq1_1,self.seq1_2,1,0.01)
            self.JC.set_eff('inner_ankle',-5)
            self.JC.send_pos_traj(self.seq1_2,self.seq1_3,1,0.01)
            rospy.sleep(0.2)
        if which == "outer":
            self.JC.send_pos_traj(self.pos1,self.seq2_1,1,0.01)
            rospy.sleep(0.2)
            self.JC.send_pos_traj(self.seq2_1,self.seq2_2,1,0.01)
            self.JC.set_eff('left_ankle',-5)
            self.JC.set_eff('right_ankle',-5)
            self.JC.send_pos_traj(self.seq2_2,self.seq2_3,1,0.01)
            rospy.sleep(0.2)

    def TakeFirstStep(self,which):
        if which == "inner":
            self.JC.set_eff('left_ankle',-1)
            self.JC.set_eff('right_ankle',-1)
            self.JC.set_eff('left_hip',3.5)
            self.JC.set_eff('right_hip',3.5)
            self.JC.set_eff('inner_ankle',0)
            self.JC.send_command()
            rospy.sleep(0.67)
            self.JC.set_eff('left_ankle',0.5)
            self.JC.set_eff('right_ankle',0.5)
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.set_eff('inner_ankle',0)
            self.JC.send_command()

        if which == "outer":
            self.JC.set_eff('left_ankle',-3)
            self.JC.set_eff('right_ankle',-3)
            self.JC.set_eff('left_hip',-2.7)
            self.JC.set_eff('right_hip',-2.7)
            self.JC.set_pos('inner_ankle',0.1)
            self.JC.send_command()
            rospy.sleep(0.1)
            #JC.set_pos('inner_ankle',0.0)
            self.JC.set_eff('left_ankle',0.1)
            self.JC.set_eff('right_ankle',0.1)
            self.JC.send_command()
            rospy.sleep(0.55)
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.set_eff('inner_ankle',0.5)
            self.JC.send_command()
            rospy.sleep(0.1)
            self.JC.set_eff('left_ankle',0)
            self.JC.set_eff('right_ankle',0)
            self.JC.send_command()

    def TakeStep(self,which):
        #TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

        Aperture = abs(self.RS.GetJointPos('left_hip'))
        SwingEff = self.SwingEff0 + self.kAp*(self.DesAp - Aperture)
            
        #if TimeElapsed<30:
        #    Aperture = abs(self.RS.GetJointPos('left_hip'))
        #    SwingEff = self.SwingEff0 + self.kAp*(self.DesAp - Aperture)
        #    #print SwingEff
        #else:
        #    if which == "outer":
        #        SwingEff = 0.20
        #    else:
        #        SwingEff = 0.28
        if which == "outer":
            # Toe off
            self.JC.set_gains('inner_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
            self.JC.set_pos('inner_ankle',self.SupAnkSetPoint)
            self.JC.set_eff('left_ankle',self.ToeOffEff)
            self.JC.set_eff('right_ankle',self.ToeOffEff)
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.send_command()
            rospy.sleep(self.ToeOffDur)
            # Raise feet and swing
            self.JC.set_eff('left_ankle',-3)
            self.JC.set_eff('right_ankle',-3)
            self.JC.set_eff('left_hip',-SwingEff)
            self.JC.set_eff('right_hip',-SwingEff)
            self.JC.send_command()
            rospy.sleep(self.TotSwingDur-self.FootExtDur-self.FreeSwingDur)
            # Lower feet
            self.JC.set_eff('left_ankle',self.FootExtEff)
            self.JC.set_eff('right_ankle',self.FootExtEff)
            self.JC.send_command()
            rospy.sleep(self.FootExtDur)
            # End swing
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.set_eff('left_ankle',0)
            self.JC.set_eff('right_ankle',0)
            self.JC.send_command()
            rospy.sleep(self.FreeSwingDur)

        if which == "inner":
            # Toe off
            self.JC.set_gains('left_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
            self.JC.set_gains('right_ankle',self.SupAnkGain_p,self.SupAnkGain_d,0,set_default = False)
            self.JC.set_pos('left_ankle',self.SupAnkSetPoint)
            self.JC.set_pos('right_ankle',self.SupAnkSetPoint)
            self.JC.set_eff('inner_ankle',self.ToeOffEff)
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.send_command()
            rospy.sleep(self.ToeOffDur)
            # Raise feet and swing
            self.JC.set_eff('inner_ankle',-3)
            self.JC.set_eff('left_hip',SwingEff)
            self.JC.set_eff('right_hip',SwingEff)
            self.JC.send_command()
            rospy.sleep(self.TotSwingDur-self.FootExtDur-self.FreeSwingDur)
            # Lower feet
            self.JC.set_eff('inner_ankle',self.FootExtEff)
            self.JC.send_command()
            rospy.sleep(self.FootExtDur)
            # End swing
            self.JC.set_eff('left_hip',0)
            self.JC.set_eff('right_hip',0)
            self.JC.set_eff('inner_ankle',0)
            self.JC.send_command()
            rospy.sleep(self.FreeSwingDur)

    def ResetPose(self):
        self.JC.set_pos('left_hip',0)
        self.JC.set_pos('left_ankle',0)
        self.JC.set_pos('right_hip',0)
        self.JC.set_pos('right_ankle',0)
        self.JC.set_pos('inner_ankle',0)
        self.JC.send_command()

    def RS_cb(self,msg):

        self.RS.UpdateState(msg)

    def Odom_cb(self,msg):
        self.GlobalPos = msg.pose.pose.position

    def reset(self):
        self.reset_srv()
        rospy.sleep(0.5)

        while self.GlobalPos.z<0.95 or self.GlobalPos.z>1.05 or abs(self.GlobalPos.x)>0.5:
            self.reset_srv()
            rospy.sleep(0.5)

    def Run(self,TimeOut = 0):
        rospy.sleep(0.1) 
        # Start with inner foot raised
        self.JC.send_pos_traj(self.RS.GetJointPos(),self.seq2_3,0.5,0.01)
        self.reset()
        rospy.sleep(1) 

        self.StartTime = rospy.Time.now().to_sec()

        self.TakeFirstStep("outer")
        self.ApertureMean = abs(self.RS.GetJointPos('left_hip'))

        self.Leg = "inner"
        self.Go=1
        while self.Go == 1:
            TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

            if self.GlobalPos.z<0.6 or self.GlobalPos.z>1.4 or (TimeElapsed>TimeOut and TimeOut>0):
                # if the robot fell, stop running and return fitness
                return self.GlobalPos, self.ApertureStd

            self.TakeStep(self.Leg)

            self.StepsTaken += 1
            ThisAperture = abs(self.RS.GetJointPos('left_hip'))
            self.ApertureMean += (ThisAperture-self.ApertureMean) / self.StepsTaken
            self.ApertureStd += ((ThisAperture-self.ApertureMean)**2 - self.ApertureStd) / self.StepsTaken

            if self.Leg == "inner":
                self.Leg = "outer"
            else:
                self.Leg = "inner"
    def __del__(self):
        self.MsgSub.unregister()
        self.OdomSub.unregister()
Beispiel #8
0
class Fivel_Controller(object):
    """Fivel_Controller"""
    def __init__(self, arg):
        super(Fivel_Controller, self).__init__()

        ##################################################################
        ######################## GAIT PARAMETERS #########################
        ##################################################################
        if len(arg) < 12:
            self.SwingEff0 = 0.44  # Default swing effort
            self.kAp = 3  # Aperture feedback gain
            self.DesAp = 0.4  # Desired aperture
            self.SupAnkGain_p = 10  # Ankle gain p during support
            self.SupAnkGain_d = 10  # Ankle gain d during support
            self.SupAnkSetPoint = -0.02  # Support ankle set point
            self.ToeOffEff0 = 3.5  # Toe-off effort
            self.ToeOffDur = 0.20  # Toe-off duration
            self.TotSwingDur = 0.85  # Swing duration
            self.FootExtEff = 0.75  # Foot extension effort (right before touch down)
            self.FootExtDur = 0.18  # Foot extension duration
            self.FreeSwingDur = 0.15  # Swing duration

            self.SwingEff = self.SwingEff0
            self.ToeOffEff = self.ToeOffEff0
        else:
            self.SwingEff0 = arg[0]  # Default swing effort
            self.kAp = arg[1]  # Aperture feedback gain
            self.DesAp = arg[2]  # Desired aperture
            self.SupAnkGain_p = arg[3]  # Ankle gain p during support
            self.SupAnkGain_d = arg[4]  # Ankle gain d during support
            self.SupAnkSetPoint = arg[5]  # Support ankle set point
            self.ToeOffEff0 = arg[6]  # Toe-off effort
            self.ToeOffDur = arg[7]  # Toe-off duration
            self.TotSwingDur = arg[8]  # Swing duration
            self.FootExtEff = arg[
                9]  # Foot extension effort (right before touch down)
            self.FootExtDur = arg[10]  # Foot extension duration
            self.FreeSwingDur = arg[11]  # Swing duration

            self.SwingEff = self.SwingEff0
            self.ToeOffEff = self.ToeOffEff0

        self.StepsTaken = 0
        self.ApertureMean = 0
        self.ApertureStd = 0
        self.GlobalPos = 0

        self.reset_srv = rospy.ServiceProxy('/gazebo/reset_models', Empty)

        ##################################################################
        ###################### RAISE FOOT SEQUENCE #######################
        ##################################################################

        self.pos1 = zeros(6)

        # "inner_hip","inner_ankle","left_hip","left_ankle","right_hip","right_ankle"
        #      0            1           2           3            4            5

        # Sequence 1 for raising the inner foot (from home position)
        self.seq1_1 = copy(self.pos1)
        self.seq1_1[3] = self.seq1_1[5] = -0.03
        self.seq1_1[0] = 0.25
        self.seq1_1[1] = -0.25

        self.seq1_2 = copy(self.seq1_1)
        self.seq1_2[1] = -1.49

        # Sequence 2 for raising the outer feet (from home position)
        self.seq2_1 = copy(self.pos1)
        self.seq2_1[1] = -0.03
        self.seq2_1[2] = self.seq2_1[4] = 0.25
        self.seq2_1[3] = self.seq2_1[5] = -0.25

        self.seq2_2 = copy(self.seq2_1)
        self.seq2_2[3] = self.seq2_2[5] = -1.49
        #self.seq2_2[1] = self.seq2_2[3] = -0.16
        #self.seq2_2[1] = 0.06

        #self.seq2_3=copy(self.seq2_2)
        #self.seq2_3[1] = self.seq2_3[3] = -0.1

        ##################################################################
        ########################## INITIALIZE ############################
        ##################################################################
        self._robot_name = "five_link_walker"
        self._jnt_names = [
            "inner_hip", "inner_ankle", "left_hip", "left_ankle", "right_hip",
            "right_ankle"
        ]
        # Initialize joint commands handler
        self.JC = JointCommands_msg_handler(self._robot_name, self._jnt_names)

        # Initialize robot state listener
        self.RS = robot_state(self._jnt_names)
        self.MsgSub = rospy.Subscriber('/' + self._robot_name + '/robot_state',
                                       RobotState, self.RS_cb)
        self.OdomSub = rospy.Subscriber('/ground_truth_odom', Odometry,
                                        self.Odom_cb)

    ##################################################################
    ########################### FUNCTIONS ############################
    ##################################################################

    def RaiseLeg(self, which):
        if which == "inner":
            self.JC.send_pos_traj(self.pos1, self.seq1_1, 1, 0.01)
            rospy.sleep(0.2)
            self.JC.send_pos_traj(self.seq1_1, self.seq1_2, 1, 0.01)
            self.JC.set_eff('inner_ankle', -5)
            self.JC.send_pos_traj(self.seq1_2, self.seq1_3, 1, 0.01)
            rospy.sleep(0.2)
        if which == "outer":
            self.JC.send_pos_traj(self.pos1, self.seq2_1, 2, 0.01)
            rospy.sleep(0.2)
            self.JC.send_pos_traj(self.seq2_1, self.seq2_2, 2, 0.01)
            self.JC.set_eff('left_ankle', -5)
            self.JC.set_eff('right_ankle', -5)
            self.JC.send_command()
            #self.JC.send_pos_traj(self.seq2_2,self.seq2_3,1,0.01)
            rospy.sleep(0.2)

    def StandOn(self, leg, time, dt):
        StartTime = rospy.Time.now().to_sec()
        while rospy.Time.now().to_sec() - StartTime < time:
            if leg == "inner":
                # Apply closed loop torque for upright torso
                hip_eff = self.BalanceCmd(leg)

                self.JC.set_eff('inner_hip', hip_eff)
                self.JC.send_command()
                rospy.sleep(dt)

    def BalanceCmd(self, which):
        if which == "inner":
            # Get angles
            ia_pos = self.RS.GetJointPos("inner_ankle")
            ih_pos = self.RS.GetJointPos("inner_hip")
            ia_vel = self.RS.GetJointVel("inner_ankle")
            ih_vel = self.RS.GetJointVel("inner_hip")

            torso_angle = ia_pos + ih_pos
            torso_angvel = ia_vel + ih_vel

            # Apply closed loop torque for upright torso
            hip_eff = 200 * (0 - torso_angle) + 50 * (0 - torso_angvel)
            return hip_eff
        if which == "outer":
            # Get angles
            la_pos = self.RS.GetJointPos("left_ankle")
            lh_pos = self.RS.GetJointPos("left_hip")
            la_vel = self.RS.GetJointVel("left_ankle")
            lh_vel = self.RS.GetJointVel("left_hip")

            ra_pos = self.RS.GetJointPos("right_ankle")
            rh_pos = self.RS.GetJointPos("right_hip")
            ra_vel = self.RS.GetJointVel("right_ankle")
            rh_vel = self.RS.GetJointVel("right_hip")

            ltorso_angle = la_pos + lh_pos
            ltorso_angvel = la_vel + lh_vel

            rtorso_angle = ra_pos + rh_pos
            rtorso_angvel = ra_vel + rh_vel

            # Apply closed loop torque for upright torso
            lhip_eff = 200 * (0 - ltorso_angle) + 50 * (0 - ltorso_angvel)
            rhip_eff = 200 * (0 - rtorso_angle) + 50 * (0 - rtorso_angvel)
            return lhip_eff, rhip_eff

    def TakeFirstStep(self, which):
        if which == "inner":
            self.JC.set_eff('left_ankle', -1)
            self.JC.set_eff('right_ankle', -1)
            self.JC.set_eff('left_hip', 3.5)
            self.JC.set_eff('right_hip', 3.5)
            self.JC.set_eff('inner_ankle', 0)
            self.JC.send_command()
            rospy.sleep(0.67)
            self.JC.set_eff('left_ankle', 0.5)
            self.JC.set_eff('right_ankle', 0.5)
            self.JC.set_eff('left_hip', 0)
            self.JC.set_eff('right_hip', 0)
            self.JC.set_eff('inner_ankle', 0)
            self.JC.send_command()

        if which == "outer":
            self.JC.set_eff('left_ankle', -3)
            self.JC.set_eff('right_ankle', -3)
            self.JC.set_eff('left_hip', -3.0)
            self.JC.set_eff('right_hip', -3.0)
            self.JC.set_pos('inner_ankle', 0.1)
            self.JC.send_command()
            rospy.sleep(0.1)
            #JC.set_pos('inner_ankle',0.0)
            self.JC.set_eff('left_ankle', 0.1)
            self.JC.set_eff('right_ankle', 0.1)
            self.JC.send_command()
            rospy.sleep(0.55)
            self.JC.set_eff('left_hip', 0)
            self.JC.set_eff('right_hip', 0)
            self.JC.set_eff('inner_ankle', 0.5)
            self.JC.send_command()
            rospy.sleep(0.1)
            self.JC.set_eff('left_ankle', 0)
            self.JC.set_eff('right_ankle', 0)
            self.JC.send_command()

    def TakeStep(self, which):
        TimeElapsed = rospy.Time.now().to_sec() - self.StartTime

        if TimeElapsed < 60:
            Aperture = abs(self.RS.GetJointPos('left_hip'))
            self.SwingEff = self.SwingEff0 + self.kAp * (self.DesAp - Aperture)
            #self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)

            if which == 'outer':
                Aperture = self.RS.GetJointPos('left_hip')
            else:
                Aperture = -self.RS.GetJointPos('left_hip')
            self.ToeOffEff = self.ToeOffEff0 - 1 * Aperture
            #self.SupAnkSetPoint = -0.02 + 0.03 * Aperture

            print Aperture
        else:
            Aperture = abs(self.RS.GetJointPos('left_hip'))
            #self.ToeOffEff = self.ToeOffEff0 - 2*(self.DesAp - Aperture)
            #print self.ToeOffEff
            #print self.SwingEff
        if which == "outer":
            # Toe off
            self.JC.set_gains('inner_ankle',
                              self.SupAnkGain_p,
                              self.SupAnkGain_d,
                              0,
                              set_default=False)
            self.JC.set_pos('inner_ankle', self.SupAnkSetPoint)
            self.JC.set_eff('left_ankle', self.ToeOffEff)
            self.JC.set_eff('right_ankle', self.ToeOffEff)
            self.JC.set_eff('left_hip', 0)
            self.JC.set_eff('right_hip', 0)
            self.JC.send_command()
            rospy.sleep(self.ToeOffDur)
            # Raise feet and swing
            self.JC.set_eff('left_ankle', -3)
            self.JC.set_eff('right_ankle', -3)
            self.JC.set_eff('left_hip', -self.SwingEff)
            self.JC.set_eff('right_hip', -self.SwingEff)
            self.JC.send_command()
            rospy.sleep(self.TotSwingDur - self.FootExtDur - self.FreeSwingDur)
            # Lower feet
            self.JC.set_eff('left_ankle', self.FootExtEff)
            self.JC.set_eff('right_ankle', self.FootExtEff)
            self.JC.send_command()
            rospy.sleep(self.FootExtDur)
            # End swing
            self.JC.set_eff('left_hip', 0)
            self.JC.set_eff('right_hip', 0)
            self.JC.set_eff('left_ankle', 0)
            self.JC.set_eff('right_ankle', 0)
            self.JC.send_command()
            rospy.sleep(self.FreeSwingDur)

        if which == "inner":
            # Toe off
            self.JC.set_gains('left_ankle',
                              self.SupAnkGain_p,
                              self.SupAnkGain_d,
                              0,
                              set_default=False)
            self.JC.set_gains('right_ankle',
                              self.SupAnkGain_p,
                              self.SupAnkGain_d,
                              0,
                              set_default=False)
            self.JC.set_pos('left_ankle', self.SupAnkSetPoint)
            self.JC.set_pos('right_ankle', self.SupAnkSetPoint)
            self.JC.set_eff('inner_ankle', self.ToeOffEff)
            self.JC.set_eff('left_hip', 0)
            self.JC.set_eff('right_hip', 0)
            self.JC.send_command()
            rospy.sleep(self.ToeOffDur)
            # Raise feet and swing
            self.JC.set_eff('inner_ankle', -3)
            self.JC.set_eff('left_hip', self.SwingEff)
            self.JC.set_eff('right_hip', self.SwingEff)
            self.JC.send_command()
            rospy.sleep(self.TotSwingDur - self.FootExtDur - self.FreeSwingDur)
            # Lower feet
            self.JC.set_eff('inner_ankle', self.FootExtEff)
            self.JC.send_command()
            rospy.sleep(self.FootExtDur)
            # End swing
            self.JC.set_eff('left_hip', 0)
            self.JC.set_eff('right_hip', 0)
            self.JC.set_eff('inner_ankle', 0)
            self.JC.send_command()
            rospy.sleep(self.FreeSwingDur)

    def ResetPose(self):
        self.JC.set_pos('left_hip', 0)
        self.JC.set_pos('left_ankle', 0)
        self.JC.set_pos('right_hip', 0)
        self.JC.set_pos('right_ankle', 0)
        self.JC.set_pos('inner_ankle', 0)
        self.JC.send_command()

    def RS_cb(self, msg):
        self.RS.UpdateState(msg)

    def Odom_cb(self, msg):
        self.GlobalPos = msg.pose.pose.position

    def reset(self):
        self.reset_srv()
        rospy.sleep(0.5)

        while self.GlobalPos.z < 0.95 or self.GlobalPos.z > 1.05 or abs(
                self.GlobalPos.x) > 0.5:
            self.reset_srv()
            rospy.sleep(0.5)

    def Run(self, TimeOut=0):
        rospy.sleep(0.1)

        pos = zeros(6)
        pos[0] = 1
        self.JC.reset_command()
        self.JC.send_pos_traj(zeros(6), pos, 0.5, 0.01)