コード例 #1
0
ファイル: G5L_Controller.py プロジェクト: xinsongyan/5link
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"
コード例 #2
0
ファイル: G5L_Controller.py プロジェクト: lothas/5link
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"
コード例 #3
0
ファイル: 5L_Controller.py プロジェクト: EricSidorov/5link
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) 
コード例 #4
0
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"
コード例 #5
0
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()
コード例 #6
0
ファイル: 5L_Controller.py プロジェクト: xinsongyan/5link
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)