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(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(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)
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()
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)
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 ResetPose():
JC.set_pos('left_hip',0)
JC.set_pos('left_ankle',0)
JC.set_pos('right_hip',0)
JC.set_pos('right_ankle',0)
JC.set_pos('inner_ankle',0)
JC.send_command()
def RS_cb(msg):
RS.UpdateState(msg)
# Initialize joint commands handler
JC = JointCommands_msg_handler()
# Initialize robot state listener
RS = robot_state()
MsgSub = rospy.Subscriber('/compass_biped/robot_state',RobotState,RS_cb)
rospy.sleep(0.1)
#JC.send_pos_traj(RS.GetJointPos(),pos1,0.5,0.01)
JC.send_pos_traj(RS.GetJointPos(),seq2_3,0.5,0.01)
reset()
#rospy.sleep(1)
#RaiseLeg("inner")
rospy.sleep(1.5)
TakeFirstStep("outer")
TakeStep("inner")
TakeStep("outer")
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)
#! /usr/bin/env python
import rospy
import robot_state
from constants import constants as C
from functions import print_ros
print_ros("Main starting...")
robostilt=robot_state.robot_state()
robostilt.set_initial_state()
#robostilt.lower_legs_on_frame(C.FRAME.EVEN,False)
robostilt.move_actuator(4,-0.2) #fall on purpose
robostilt.raise_frame_while_leveling(-1.0)
# while(True):
# robostilt.step_forward()
# #print_ros("Done with stepForward")
rospy.spin()
