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"
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)
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"
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"
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()
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)