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)
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()
#!/usr/bin/env python
import roslib;roslib.load_manifest('C42_DynamicLocomotion')
import rospy
from JointController import JointCommands_msg_handler
from robot_state import robot_state
class stand_up_controller(object):
def __init__(self,JC,RS,Tf):
self.JC = JC
self.RS = RS
self.Tf = Tf
def extend(self):
pass
def rs_cb(self,msg):
self.RS.UpdateState(msg)
if __name__ == '__main__':
robot_name = "atlas"
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
JC = JointCommands_msg_handler(robot_name,self._jnt_names)
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)
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)
