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)