def __init__(self): self.num_channels = 2 self.is_running = False #Create the servoabsolute msg publisher self.servo_msg = Servo() for i in range(self.num_channels): self.servo_msg.servos.append(Servo()) self.servo_msg.servos(i).servo = i + 1 self.pubmsg = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1)
def __init__(self): rospy.loginfo("Setting up Donkeycar Node...") rospy.init_node('donkeycar') """ Create actuator dictionary { throttle: ServoConvert(id=1) steer: ServoConvert(id=2) } """ self.actuators = {} self.actuators['throttle'] = ServoConvert(id=1) self.actuators['steering'] = ServoConvert(id=2) rospy.loginfo("> Actuators corrrectly initialized") # Create servo array # 2 servos - 1 = Throttle | 2 = Steer self._servo_msg = ServoArray() for i in range(2): self._servo_msg.servos.append(Servo()) # Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") # Create the Subscriber to Joystick commands self.ros_sub_twist = rospy.Subscriber("/joy", Joy, self.set_actuators_from_joystick) rospy.loginfo("> Subscriber corrrectly initialized") rospy.loginfo("Initialization complete")
def __init__(self): rospy.loginfo("Setting up low-level control node...") rospy.init_node("dk_llc") # Create an actuator dictionary self.actuators = {} self.actuators['throttle'] = ServoConvert(id=1) self.actuators['steering'] = ServoConvert( id=2, direction=1) # Sets positive direction left # Create the servo arra publisher and message self._servo_msg = ServoArray() for i in range(2): # One for speed, one for steering self._servo_msg.servos.append(Servo()) self.pub = rospy.Publisher('/servos_absolute', ServoArray, queue_size=1) rospy.loginfo('> LLC Publisher correctly initialized') # Create subscriber to the /cmd_vel topic self.sub_cmd_vel = rospy.Subscriber('/cmd_vel', Twist, self.set_actuators_from_cmdvel) rospy.loginfo('> LLC Subscriber correctly initialized') self._last_time_cmd_rcv = time.time() self._timeout_s = 5 # Stop after this amount of seconds if no additional commands received rospy.loginfo('LLC initialization complete!')
def __init__(self): rospy.loginfo("Setting Up the Spot Micro Servo Control Node...") # Set up and title the ros node for this code rospy.init_node('spot_micro_servo_control') # Intialize empty servo dictionary self.servos = {} # Create a servo dictionary with 12 ServoConvert objects # keys: integers 0 through 12 # values: ServoConvert objects for i in range(numServos): self.servos[i] = ServoConvert(id=i) rospy.loginfo("> Servos corrrectly initialized") # Create empty ServoArray message with n number of Servos in its array self._servo_msg = ServoArray() for i in range(numServos): self._servo_msg.servos.append(Servo()) # Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") rospy.loginfo("Initialization complete") # Setup terminal input reading, taken from teleop_twist_keyboard self.settings = termios.tcgetattr(sys.stdin)
def __init__(self): rospy.loginfo("Setting up the Node...") rospy.init_node('dk_llc') ##--- Create an actuator dictionary self.actuators = {} self.actuators['throttle'] = ServoConvert(id = 1) self.actuators['steering'] = ServoConvert(id = 2, direction = 1) ##- Positive left rospy.loginfo("> Actuators correctly initialized!") ##--- Create the servo array publisher ##-- Create the message self._servo_msg = ServoArray() for i in range(2): self._servo_msg.servos.append(Servo()) ##--- Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servo_absolute", ServoArray, queue_size = 1) rospy.loginfo("> Publisher correctly initialized!") ##--- Create the Subscriber to Twist commands - /cmd_vel topic self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.set_actuators_from_cmdvel) rospy.loginfo("> Subscriber correctly initalized!") ##--- Save the last time we got a reference. Stop the vehicle self._last_time_cmd_rcv = time.time() self._timeout_s = 5 ##-- Stop after 5 seconds rospy.loginfo("Initialization complete!")
def __init__(self): rospy.loginfo("Setting Up the Node...") rospy.init_node('dk_llc') self.actuators = {} self.actuators['throttle'] = ServoConvert(id=1) self.actuators['steering'] = ServoConvert( id=2, direction=1) #-- positive left rospy.loginfo("> Actuators corrrectly initialized") self._servo_msg = ServoArray() for i in range(2): self._servo_msg.servos.append(Servo()) #--- Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") #--- Create the Subscriber to Twist commands self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.set_actuators_from_cmdvel) rospy.loginfo("> Subscriber corrrectly initialized") #--- Get the last time e got a commands self._last_time_cmd_rcv = time.time() self._timeout_s = 5 rospy.loginfo("Initialization complete")
def __init__(self): rospy.loginfo("Setting up the node ...") rospy.init_node("kbrd_act") #--- Create an actuator dictionary self.actuators = {} self.actuators['throttle'] = ServoConvert(id=1) self.actuators['steering'] = ServoConvert(id=2, direction=1) #-positive left #--- Create servo array publisher #-- Create the message self._servo_msg = ServoArray() for i in range(2): self._servo_msg.servos.append(Servo()) self._ros_pub_servo_array = rospy.Publisher("servos_absolute", ServoArray, queue_size=1) rospy.loginfo("> Publisher correctly initialized") #--- Create the subscriber to the /cmd_vel topic self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.set_actuators_from_cmdvel) rospy.loginfo("> Subscriber correctly initialized") #--- Save the last time we got a correspondence. Stop the vehicle self._last_time_cmd_rcv = time.time() self._timeout_s = 5 #-- stop after 5 sec rospy.loginfo("Initialization complete")
def __init__(self): rospy.loginfo("Setting Up the Node...") rospy.init_node('robot_llc') self.actuators = {} self.actuators['shoulder_1'] = ServoConvert(id=1) self.actuators['shoulder_2'] = ServoConvert(id=2) self.actuators['elbow_1'] = ServoConvert(id=3) self.actuators['elbow_2'] = ServoConvert(id=4) self.actuators['wrist_1'] = ServoConvert(id=5) self.actuators['wrist_2'] = ServoConvert(id=6) rospy.loginfo("> Actuators corrrectly initialized") self._servo_msg = ServoArray() for i in range(16): self._servo_msg.servos.append(Servo()) #--- Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") #--- Create the Subscriber to Twist commands self.ros_sub_value = rospy.Subscriber("/cmd_vel", Twist, self.set_value) rospy.loginfo("> Subscriber corrrectly initialized") self.ros_sub_motions=rospy.Subscriber("/execute_motion", String ,self.set_motion) rospy.loginfo("Initialization complete")
def ControleMoteur(Va,Vb): pub = rospy.Publisher('/servos_absolute', ServoArray,queue_size=100) rospy.init_node('ControleMoteur', anonymous=True) rate = rospy.Rate(50) # 10hz msg=ServoArray() MoteurA=Servo() MoteurA.servo=16 MoteurA.value=0 MoteurB=Servo() MoteurB.servo=15 MoteurB.value=0 msg.servos=[MoteurA, MoteurB] while not rospy.is_shutdown(): rospy.loginfo(msg) pub.publish(msg) rate.sleep()
class SpotMicroServoControl(): def __init__(self): # Create a ServoConfig messag for one servo self._servo_config_msg = ServoConfigArray() self._servo1_config = ServoConfig() self._servo1_config.center = 300 self._servo1_config.range = 400 self._servo1_config.servo = 1 self._servo1_config.direction = 1 self._servo_config_msg.servos.append(self._servo1_config) rospy.loginfo("> Waiting for config_servos service...") print('test1') rospy.wait_for_service('config_servos') rospy.loginfo("> Config_servos service found!") print('test2') try: servoConfigService = rospy.ServiceProxy('config_servos', ServosConfig) resp = servoConfigService(self._servo_config_msg.servos) print("Config servos done!!, returned value: %i" % resp.error) except rospy.ServiceException, e: print "Service call failed: %s" % e rospy.loginfo("Setting Up the Spot Micro Servo Control Node...") # Set up and title the ros node for this code rospy.init_node('spot_micro_servo_control') # Intialize empty servo dictionary self.servos = {} # Create a servo dictionary with 12 ServoConvert objects # keys: integers 0 through 12 # values: ServoConvert objects for i in range(numServos): self.servos[i] = ServoConvert(id=i) rospy.loginfo("> Servos corrrectly initialized") # Create empty ServoArray message with n number of Servos in its array self._servo_msg = ServoArray() for i in range(numServos): self._servo_msg.servos.append(Servo()) # Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servos_proportional", ServoArray, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") rospy.loginfo("Initialization complete") # Setup terminal input reading, taken from teleop_twist_keyboard self.settings = termios.tcgetattr(sys.stdin)
def __init__(self, pwm1, pwm2, pwm3): self.pub = rospy.Publisher('/servos_absolute', ServoArray, queue_size=100) rospy.init_node('ControleMoteur', anonymous=True) rospy.on_shutdown(self.arret) self.rate = rospy.Rate(50) # 10hz self.msg = ServoArray() self.Servo1 = Servo() self.Servo2 = Servo() self.Servo3 = Servo() # DC moteurs self.Servo1.servo = pwm1 self.Servo1.value = 0 self.Servo2.servo = pwm2 self.Servo2.value = 0 # Servo moteur self.Servo3.servo = pwm3 self.Servo3.value = 0 self.msg.servos = [self.Servo1, self.Servo2, self.Servo3]
class donkey_driver: #initiate the servo control board and set the dc driver to zero def __init__(self): self.num_channels = 2 self.is_running = False #Create the servoabsolute msg publisher self.servo_msg = Servo() for i in range(self.num_channels): self.servo_msg.servos.append(Servo()) self.servo_msg.servos(i).servo = i+1 self.pubmsg = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1)
def __init__(self): rospy.loginfo("Setting Up the Node...") #--- Initialize the node rospy.init_node('dk_llc') self.actuators = {} self.actuators['throttle'] = ServoConvert(id=1) self.actuators['steering'] = ServoConvert( id=2, center_value=328, direction=1) #-- positive left rospy.loginfo("> Actuators corrrectly initialized") self._servo_msg = ServoArray() for i in range(2): self._servo_msg.servos.append(Servo()) #--- Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") #--- Create a debug publisher for resulting cmd_vel self.ros_pub_debug_command = rospy.Publisher("/dkcar/debug/cmd_vel", Twist, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") #--- Create the Subscriber to Twist commands self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.update_message_from_command) rospy.loginfo("> Subscriber corrrectly initialized") #--- Create the Subscriber to obstacle_avoidance commands self.ros_sub_twist = rospy.Subscriber("/dkcar/control/cmd_vel", Twist, self.update_message_from_avoid) rospy.loginfo("> Subscriber corrrectly initialized") self.throttle_cmd = 0. self.throttle_avoid = 1. self.steer_cmd = 0. self.steer_avoid = 0. self._debud_command_msg = Twist() #--- Get the last time e got a commands self._last_time_cmd_rcv = time.time() self._last_time_avoid_rcv = time.time() self._timeout_s = 5 rospy.loginfo("Initialization complete")
def __init__(self): #Inicializar motores y servo self.actuators = {} self.actuators['MotorDerecho'] = ServoConvert(id=14) self.actuators['MotorIzquierdo'] = ServoConvert(id=13) self.actuators['ServoBasura']= ServoConvert(id=16) self._servo_msg = ServoArray() for i in range(16): self._servo_msg.servos.append(Servo()) self._ros_sub_aruco = rospy.Subscriber("/fiducial_vertices", FiducialArray, self.callback) self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) self.idAruco=0 #Inicializar seguidor de linea GPIO.setmode(GPIO.BCM) self.wp = wp self.lastError=0 self.errorI=0 self.lastProporcional=0 self.integral=0 self.KP=2 #self.KI=0.001 self.KD=5 self.velMinIzquierda=1500 self.velMinDerecha=1500 self.velMaxIzquierda=2500 self.velMaxDerecha=2500 self.Foco=20 self.Boton=6 GPIO.setup(self.Foco, GPIO.OUT) GPIO.output(self.Foco, GPIO.HIGH) self.LEDON_PIN = 5 self.SENSOR_PINS = [26, 23, 24, 25, 12]#6 self.NUM_SENSORS = len(self.SENSOR_PINS) self.CHARGE_TIME = 10 #us to charge the capacitors self.READING_TIMEOUT = 1000 #us, assume reading is black self.sensorValues = [] self.calibratedMax = [] self.calibratedMin = [] self.lastValue = 0 self.init_pins()
def __init__(self): rospy.loginfo("Setting Up the Node...") rospy.init_node('dk_llc') self._servo_msg = ServoArray() for i in range(16): self._servo_msg.servos.append(Servo()) #--- Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") #--- Create the Subscriber to Twist commands self.ros_sub_value = rospy.Subscriber("/cmd_vel", Twist, self.set_value) rospy.loginfo("> Subscriber corrrectly initialized") rospy.loginfo("Initialization complete")
def __init__(self): rospy.loginfo("Setting up the node...") rospy.init_node("ts_llc") motor.setup() motor.ctrl(1) #--- Create the actuator dictionary self.actuators = {} self.actuators['throttle'] = ServoConvert(id=1, centre_value=0) motor.setSpeed(0) self.actuators['steering'] = ServoConvert(id=2, direction=1) #-positive left #--- Create the servo array publisher #-- Create the message self._servo_msg = ServoArray() for i in range(2): self._servo_msg.servos.append(Servo()) self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) rospy.loginfo("> Publisher correctly initialized") #--- Create the subscriber to the /cmd-vel topic self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.set_actuator_from_cmdvel) #self.motor_sub = rospy.Subscriber("/cmd_vel", Twist, self.move_dc_from_cmdvel) rospy.loginfo("> subscriber correctly initialized") #--- save the last time we got a reference. Stop the vehicle if no commands given self._last_time_cmd_rcv = time.time() self._timeout_s = 5 #-- stop after 5 seconds rospy.loginfo("Initialization complete")
# 1. Direction and velocity commands for Planning (geometry # and chaosbot hardward specific). # 2. Button commands for actions like Emergency stop # and gripper movements # 3. Button commands for poses or autonomous motions # # Direction and velocity geometry will be provided by # the following joystick axes. # NOTE: Using MODE where the MODE button light is ON # axis 3 aka right stick vertical controls linear speed # axis 2 aka right stick horizontal controls angular speed ARMMOTORS_IN_CONTROL = 4 joystick = Twist() chaosbot_cmds = [Servo() for i in range(ARMMOTORS_IN_CONTROL)] # Initialize a inAutonomous variable and initialize [Toggled with Button 10] inAutonomous = False global pos_wrist, pos_hand def callback_allcurrentinputs(data): rospy.loginfo('Received a joystick input') # Initialize the wrist and hand to neutral (middle) position global pos_wrist, pos_hand # Stop all motors and don't send servo positions if Button 10 if data.buttons[9] <> 0: toggleAutonomous()
class SpotMicroSimpleCommand(): '''Class to encapsulate simple command of spot micro robot''' def __init__(self): '''Constructor''' # Create and publish servo config message # Initialize servo_config_msg self._servo_config_msg = ServoConfigArray() for s in servo_dict.values(): temp_servo = ServoConfig() temp_servo.center = s['center'] temp_servo.range = s['range'] temp_servo.servo = s['num'] temp_servo.direction = s['direction'] # Append servo to servo config message self._servo_config_msg.servos.append(temp_servo) # Publish servo configuration rospy.loginfo("> Waiting for config_servos service...") rospy.wait_for_service('config_servos') rospy.loginfo("> Config_servos service found!") try: servoConfigService = rospy.ServiceProxy('config_servos',ServosConfig) resp = servoConfigService(self._servo_config_msg.servos) print("Config servos done!!, returned value: %i"%resp.error) except rospy.ServiceException, e: print "Service call failed: %s"%e # Set up and initialize ros node rospy.loginfo("Setting Up the Spot Micro Simple Command Node...") # Set up and title the ros node for this code rospy.init_node('spot_micro_simple_command') # Create a servo command dictionary in the same order as angles are received back from # SpotMicroStickFigure.get_leg_angles self.servo_cmds_rad = {'RB_1':0,'RB_2':0,'RB_3':0, 'RF_1':0,'RF_2':0,'RF_3':0, 'LF_1':0,'LF_2':0,'LF_3':0, 'LB_1':0,'LB_2':0,'LB_3':0} # Create empty ServoArray message with n number of Servos in its array self._servo_msg = ServoArray() for _ in range(len(servo_dict)): self._servo_msg.servos.append(Servo()) # Create the servo array publisher self.ros_pub_servo_array = rospy.Publisher("/servos_proportional", ServoArray, queue_size=1) rospy.loginfo("> Publisher corrrectly initialized") rospy.loginfo("Initialization complete") # Create a spot micro stick figure object to encapsulate robot state self.sm = SpotMicroStickFigure(y=0.18) # Set absolute foot positions l = self.sm.body_length w = self.sm.body_width l1 = self.sm.hip_length desired_p4_points = np.array([ [-l/2, 0, w/2 + l1], [ l/2 , 0, w/2 + l1], [ l/2 , 0, -w/2 - l1], [-l/2 , 0, -w/2 - l1] ]) self.sm.set_absolute_foot_coordinates(desired_p4_points)
def __init__(self, wheel_distance=0.6, wheel_diameter=0.4): rospy.loginfo("Setting Up the Node...") rospy.init_node('scooby_llc') GPIO.setmode(GPIO.BCM) self.PIN_LIGHT = 5 self.PIN_HORN = 6 self.gain = 0.75 GPIO.setup(self.PIN_LIGHT, GPIO.OUT) GPIO.setup(self.PIN_HORN, GPIO.OUT) GPIO.output(self.PIN_LIGHT, GPIO.HIGH) GPIO.output(self.PIN_HORN, GPIO.HIGH) self._wheel_distance = wheel_distance self._wheel_radius = wheel_diameter / 2.0 self.actuators = {} self.actuators['left_a'] = ServoConvert(id=1) self.actuators['left_b'] = ServoConvert(id=2, direction=1) #-- positive left self.actuators['right_a'] = ServoConvert(id=3) self.actuators['right_b'] = ServoConvert(id=4, direction=1) rospy.loginfo("> Actuators corrrectly initialized") self._servo_msg = ServoArray() #self._imu_data_msg = Imu() self._joint_stat = JointState() self._velocity_msg = Float32MultiArray() for i in range(4): self._servo_msg.servos.append(Servo()) #--- Create the servo array publisher self.ros_pub_velocity_array = rospy.Publisher("/wheel_velocity", Float32MultiArray, queue_size=1) self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) #self.ros_pub_imu_data = rospy.Publisher("/imu/data_raw", Imu, queue_size=1) self.ros_pub_joint_stat = rospy.Publisher("/joint_stat", JointState, queue_size=10) rospy.loginfo("> Publisher corrrectly initialized") #--- Create imu subscriber #self.ros_sub_imu = rospy.Subscriber("/rtimulib_node/imu", Imu, self.send_imu_data_msg) #--- Create the Subscriber to Joy commands self.ros_sub_joy = rospy.Subscriber("/joy_teleop/joy", Joy, self.set_actuators_from_joy) #--- Create the Subscriber to Twist commands self.ros_sub_twist = rospy.Subscriber("/joy_teleop/cmd_vel", Twist, self.set_actuators_from_cmdvel) rospy.loginfo("> Subscriber corrrectly initialized") #--- Get the last time e got a commands self._last_time_cmd_rcv = time.time() self._timeout_s = 5 self.anglez = 0.00 rospy.loginfo("Initialization complete")
def __init__(self): self.num_channels = 2 self.is_running = False #Create the servoabsolute msg publisher self.servo_msg = Servo()
def __init__(self): '''Constructor''' # Create speed, body rate, and state command data variables self.x_speed_cmd_mps = 0 self.y_speed_cmd_mps = 0 self.yaw_rate_cmd_rps = 0 self.trot_event_cmd = False self.prev_trot_event_cmd = False # Create and publish servo config message # Initialize servo_config_msg self._servo_config_msg = ServoConfigArray() for s in servo_dict.values(): temp_servo = ServoConfig() temp_servo.center = s['center'] temp_servo.range = s['range'] temp_servo.servo = s['num'] temp_servo.direction = s['direction'] # Append servo to servo config message self._servo_config_msg.servos.append(temp_servo) # Publish servo configuration # rospy.loginfo("> Waiting for config_servos service...") # rospy.wait_for_service('config_servos') # rospy.loginfo("> Config_servos service found!") # try: # servoConfigService = rospy.ServiceProxy('config_servos',ServosConfig) # resp = servoConfigService(self._servo_config_msg.servos) # print("Config servos done!!, returned value: %i"%resp.error) # except rospy.ServiceException, e: # print "Service call failed: %s"%e # Set up and initialize ros node # rospy.loginfo("Setting Up the Spot Micro Simple Command Node...") # Set up and title the ros node for this code # rospy.init_node('spot_micro_walk') # Create a servo command dictionary in the same order as angles are received back from # SpotMicroStickFigure.get_leg_angles self.servo_cmds_rad = {'RB_1':0,'RB_2':0,'RB_3':0, 'RF_1':0,'RF_2':0,'RF_3':0, 'LF_1':0,'LF_2':0,'LF_3':0, 'LB_1':0,'LB_2':0,'LB_3':0} # Create empty ServoArray message with n number of Servos in its array self._servo_msg = ServoArray() for _ in range(len(servo_dict)): self._servo_msg.servos.append(Servo()) # Create the servo array publisher # self.ros_pub_servo_array = rospy.Publisher("/servos_proportional", ServoArray, queue_size=1) # rospy.loginfo("> Publisher corrrectly initialized") # Create subsribers for speed and body rate command topics, both using vector3 # rospy.Subscriber('x_speed_cmd',Float32,self.update_x_speed_cmd) # rospy.Subscriber('/y_speed_cmd',Float32,self.update_y_speed_cmd) # rospy.Subscriber('/yaw_rate_cmd',Float32,self.update_yaw_rate_cmd) # rospy.Subscriber('/state_cmd',Bool,self.update_state_cmd) # rospy.loginfo("Initialization complete") # Create a spot micro stick figure object to encapsulate robot state self.default_height = 0.18 self.sm = SpotMicroStickFigure(y=self.default_height) # Set absolute foot positions for default stance, # foot order: RB, RF, LF, LB l = self.sm.body_length w = self.sm.body_width l1 = self.sm.hip_length self.default_sm_foot_position = np.array([ [-l/2, 0, w/2 + l1], [ l/2 , 0, w/2 + l1], [ l/2 , 0, -w/2 - l1], [-l/2 , 0, -w/2 - l1] ]) self.sm.set_absolute_foot_coordinates(self.default_sm_foot_position) # Create configuration object and update values to reflect spot micro configuration self.config = Configuration() self.config.delta_x = l/2 self.config.delta_y = w/2 + l1 self.default_z_ref = -self.default_height # Create controller object self.controller = Controller(self.config) # Create state object self.state = State() self.state.foot_locations = (self.config.default_stance + np.array([0,0,-self.default_height])[:, np.newaxis]) # Create Command object self.command = Command() self.command.height = -self.default_height