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)