def __init__(self, act_math, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10, backlash=0.0): """ act_math is the ActuatorMathHelper that contains the description of the joint and actuator kp = proportional gain term ki = integral gain term kd = derivative gain term kff= feed forward velocity term. Applies signal relative to dervative of the target derivative_corner = corner frequency of derivative filter. Our real world signals are too noisy to use without significant filtering kfa= feed forward acceleration term. Applies signal relative to the difference in rate of change of the target and desired. backlash = if we are within this angle of the target, we apply no corrective signal. """ self.act_math = act_math self.updateGainConstants(kp, ki, kd, kff, kfa) self.backlash = backlash # NOTE: it is important that these three variables are floating point to avoid truncation self.prev_error = 0.0 self.prev_desired_ang = 0.0 self.prev_ang = 0.0 self.integral_error_accumulator = 0.0 self.d_lowpass = LowPassFilter( gain=1.0, corner_frequency=derivative_corner ) self.arate_lowpass = LowPassFilter( gain=1.0, corner_frequency=derivative_corner )
class rateEstimator(object): def __init__( self ): self.last_state = 0.0 self.vel_est = LowPassFilter( gain=1.0, corner_frequency=10.0 ) def update( new_state ): return self.vel_est.update((new_state-self.last_state)/global_time.getDelta()) def getVal( self ): return self.vel_est.getVal()
class rateEstimator(object): def __init__(self): self.last_state = 0.0 self.vel_est = LowPassFilter(gain=1.0, corner_frequency=10.0) def update(new_state): return self.vel_est.update( (new_state - self.last_state) / global_time.getDelta()) def getVal(self): return self.vel_est.getVal()
def discoverDeadband( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ): # Have we made it through all the joints? if not len(self.dof_list): # FIXME: Exit safely self.file_out.close() self.callback = self.pause if self.dof_list[0][:-2] == 'hip_pitch': pos = hip_pitch cmd_i = 1 elif self.dof_list[0][:-2] == 'knee_pitch': pos = knee_pitch cmd_i = 2 elif self.dof_list[0][:-2] == 'hip_yaw': pos = yaw cmd_i = 0 else: raise if self.dof_list[0][-1] == 'e': sign = 1 elif self.dof_list[0][-1] == 'r': sign = -1 else: raise # Default command: command no movement on all valves # valves are yaw, pitch, knee, in that order # Scale depends on the mode of the underlying layer... # sometimes it expects a length rate in meters/sec, # sometimes it expects a command value between -255 and 255 cmd = [0.0,0.0,0.0] # Is this our first tick on a new DOF? if self.firstrun: # Initialize state variables print "Initializing for %s"%self.dof_list[0] self.pwm_val = 0 self.vel_IIR = LowPassFilter( gain=1.0, corner_frequency=.5 ) self.firstrun = False else: # Calculate velocity, update filter dt = time-self.last_t dpos = pos - self.last_pos vel_est = dpos/dt self.vel_IIR.update(vel_est) # Take 10 seconds to ramp valve command self.pwm_val += dt*0.05 cmd[cmd_i] = sign*self.pwm_val self.last_t = time self.last_pos = pos # Are we moving faster than our threshold? if sign*self.vel_IIR.getVal() > self.MOVE_THRESH: # If so, note the PWM value we had to apply and prepare to move the # next joint self.file_out.write( "%s: %f\n"%(self.dof_list[0],self.pwm_val) ) print "%s: %f"%(self.dof_list[0],self.pwm_val) self.dof_list.pop(0) self.firstrun = True self.callback = self.pause return cmd
def __init__(self, act_math, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10, backlash=0.0): """ act_math is the ActuatorMathHelper that contains the description of the joint and actuator kp = proportional gain term ki = integral gain term kd = derivative gain term kff= feed forward velocity term. Applies signal relative to dervative of the target derivative_corner = corner frequency of derivative filter. Our real world signals are too noisy to use without significant filtering kfa= feed forward acceleration term. Applies signal relative to the difference in rate of change of the target and desired. backlash = if we are within this angle of the target, we apply no corrective signal. """ self.act_math = act_math self.updateGainConstants(kp, ki, kd, kff, kfa) self.backlash = backlash # NOTE: it is important that these three variables are floating point to avoid truncation self.prev_error = 0.0 self.prev_desired_ang = 0.0 self.prev_ang = 0.0 self.integral_error_accumulator = 0.0 self.d_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner) self.arate_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner)
class Gait: def __init__(self): self.file_out = file('offsets.txt', 'w+') self.vel_IIR = None self.pwm_val = 0 self.last_pos = 0 self.last_t = 0 self.MOVE_THRESH = .01 self.firstrun = True self.dof_list = [] self.dof_list.append('hip_pitch_e') self.dof_list.append('hip_pitch_r') self.dof_list.append('knee_pitch_e') self.dof_list.append('knee_pitch_r') self.dof_list.append('hip_yaw_e') self.dof_list.append('hip_yaw_r') self.callback = self.moveToInitialPosition self.model = LegModel() self.controller = LimbController() def update( self, *args, **kwargs ): global_time.updateTime(args[0]) return self.callback(*args, **kwargs) def moveToInitialPosition( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ): self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) if not hasattr(self, 'initial_path'): print "Starting move to initial position" self.initial_path = TrapezoidalFootMove(self.model, self.controller,\ array([1.6, 0.0, -1.15]),\ 0.4, 0.2) self.controller.update(self.model.getJointAngles(), self.initial_path.update()) if self.initial_path.isDone(): self.callback = self.pause lr = self.controller.getLengthRateCommands() return lr def pause( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ): if not hasattr( self, 'start_pause_time' ): print "Waiting..." self.start_pause_time = time #self.pause_path = Pause( self.model, self.controller, 3.0 ) #self.controller.update(self.model.getJointAngles(), self.pause_path.update()) if time - self.start_pause_time > 3.0: self.callback = self.discoverDeadband del self.start_pause_time return [0.0,0.0,0.0] def discoverDeadband( self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None ): # Have we made it through all the joints? if not len(self.dof_list): # FIXME: Exit safely self.file_out.close() self.callback = self.pause if self.dof_list[0][:-2] == 'hip_pitch': pos = hip_pitch cmd_i = 1 elif self.dof_list[0][:-2] == 'knee_pitch': pos = knee_pitch cmd_i = 2 elif self.dof_list[0][:-2] == 'hip_yaw': pos = yaw cmd_i = 0 else: raise if self.dof_list[0][-1] == 'e': sign = 1 elif self.dof_list[0][-1] == 'r': sign = -1 else: raise # Default command: command no movement on all valves # valves are yaw, pitch, knee, in that order # Scale depends on the mode of the underlying layer... # sometimes it expects a length rate in meters/sec, # sometimes it expects a command value between -255 and 255 cmd = [0.0,0.0,0.0] # Is this our first tick on a new DOF? if self.firstrun: # Initialize state variables print "Initializing for %s"%self.dof_list[0] self.pwm_val = 0 self.vel_IIR = LowPassFilter( gain=1.0, corner_frequency=.5 ) self.firstrun = False else: # Calculate velocity, update filter dt = time-self.last_t dpos = pos - self.last_pos vel_est = dpos/dt self.vel_IIR.update(vel_est) # Take 10 seconds to ramp valve command self.pwm_val += dt*0.05 cmd[cmd_i] = sign*self.pwm_val self.last_t = time self.last_pos = pos # Are we moving faster than our threshold? if sign*self.vel_IIR.getVal() > self.MOVE_THRESH: # If so, note the PWM value we had to apply and prepare to move the # next joint self.file_out.write( "%s: %f\n"%(self.dof_list[0],self.pwm_val) ) print "%s: %f"%(self.dof_list[0],self.pwm_val) self.dof_list.pop(0) self.firstrun = True self.callback = self.pause return cmd
def __init__(self): self.last_state = 0.0 self.vel_est = LowPassFilter(gain=1.0, corner_frequency=10.0)
if type(retval) == tuple: update, controller = retval else: update = retval controller = None leg1 = ControlBus(device='/dev/ttyUSB1') yaw_valve = ValveNode(leg1 , node_id=1 , name="yaw_valve" , bore=0.0254 , rod=0.0189 , lpm=22.712 , e_deadband=89 , r_deadband=75) pitch_valve = ValveNode(leg1 , node_id=2 , name="pitch_valve" , bore=0.0381 , rod=0.0254 , lpm=22.712 , e_deadband=77 , r_deadband=70) knee_valve = ValveNode(leg1 , node_id=3 , name="knee_valve" , bore=0.0254 , rod=0.0159 , lpm=22.712 , e_deadband=87 , r_deadband=69) yaw_encoder = EncoderNode(leg1 , node_id=4 , name="yaw_encoder" , gain=1 , offset=35 * pi / 180 , bias=(1200 , 1200)) pitch_encoder = EncoderNode(leg1 , node_id=5 , name="pitch_encoder" , gain=-1 , offset=45 * pi / 180 , bias=(1200 , 1200)) knee_encoder = EncoderNode(leg1 , node_id=6 , name="knee_encoder" , gain=1 , offset=225 * pi / 180 , bias=(1200 , 1200)) shock_encoder = EncoderNode(leg1 , node_id=7 , name="shock_encoder" , gain=1 , offset=0 , bias=(1200 , 1200)) pitch_filt = LowPassFilter(gain=1.0, corner_frequency=20) yaw_filt = LowPassFilter(gain=1.0, corner_frequency=20) # yaw_cmd_filt = LowPassFilter( gain=1.0, corner_frequency=100 ) publisher = Publisher(5055) publisher.addToCatalog('time', time.time) publisher.addToCatalog('yaw_ang', yaw_encoder.getAngleDeg) publisher.addToCatalog('yaw_ang_filt', lambda: 180 * yaw_filt.getVal() / pi) publisher.addToCatalog('yaw_sin', yaw_encoder.getSinValue) publisher.addToCatalog('yaw_cos', yaw_encoder.getCosValue) publisher.addToCatalog('pitch_ang', pitch_encoder.getAngleDeg) publisher.addToCatalog('pitch_ang_filt', lambda: 180 * pitch_filt.getVal() / pi) publisher.addToCatalog('pitch_sin', pitch_encoder.getSinValue) publisher.addToCatalog('pitch_cos', pitch_encoder.getCosValue) publisher.addToCatalog('knee_ang', knee_encoder.getAngleDeg) publisher.addToCatalog('knee_sin', knee_encoder.getSinValue)
print "Hats: ", stick.get_numhats() print "Buttons: ", stick.get_numbuttons() stick_neutrals = [] d = {'offset': (0, 0, 1.5)} s = Simulator(dt=2e-3, plane=1, pave=0, graphical=1, ground_grade=.00, robot=SpiderWHydraulics, robot_kwargs=d, render_objs=1, draw_contacts=0) last_t = 0 x_low = LowPassFilter(gain=1.0, corner_frequency=1.2) y_low = LowPassFilter(gain=1.0, corner_frequency=1.2) z_low = LowPassFilter(gain=1.0, corner_frequency=1.2) r_low = LowPassFilter(gain=1.0, corner_frequency=1.2) while True: s.step() global_time.updateTime(s.getSimTime()) if s.getSimTime() - last_t > .001: x = -(stick.get_axis(1) + .15466) if x < 0: x *= 2 x /= .7 y = -(stick.get_axis(0) + .15466) if y < 0: y *= 2 y /= .7
class Gait: def __init__(self): self.file_out = file('offsets.txt', 'w+') self.vel_IIR = None self.pwm_val = 0 self.last_pos = 0 self.last_t = 0 self.MOVE_THRESH = .01 self.firstrun = True self.dof_list = [] self.dof_list.append('hip_pitch_e') self.dof_list.append('hip_pitch_r') self.dof_list.append('knee_pitch_e') self.dof_list.append('knee_pitch_r') self.dof_list.append('hip_yaw_e') self.dof_list.append('hip_yaw_r') self.callback = self.moveToInitialPosition self.model = LegModel() self.controller = LimbController() def update(self, *args, **kwargs): global_time.updateTime(args[0]) return self.callback(*args, **kwargs) def moveToInitialPosition(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): self.model.setSensorReadings(yaw, hip_pitch, knee_pitch, shock_depth) if not hasattr(self, 'initial_path'): print "Starting move to initial position" self.initial_path = TrapezoidalFootMove(self.model, self.controller,\ array([1.6, 0.0, -1.15]),\ 0.4, 0.2) self.controller.update(self.model.getJointAngles(), self.initial_path.update()) if self.initial_path.isDone(): self.callback = self.pause lr = self.controller.getLengthRateCommands() return lr def pause(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): if not hasattr(self, 'start_pause_time'): print "Waiting..." self.start_pause_time = time #self.pause_path = Pause( self.model, self.controller, 3.0 ) #self.controller.update(self.model.getJointAngles(), self.pause_path.update()) if time - self.start_pause_time > 3.0: self.callback = self.discoverDeadband del self.start_pause_time return [0.0, 0.0, 0.0] def discoverDeadband(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): # Have we made it through all the joints? if not len(self.dof_list): # FIXME: Exit safely self.file_out.close() self.callback = self.pause if self.dof_list[0][:-2] == 'hip_pitch': pos = hip_pitch cmd_i = 1 elif self.dof_list[0][:-2] == 'knee_pitch': pos = knee_pitch cmd_i = 2 elif self.dof_list[0][:-2] == 'hip_yaw': pos = yaw cmd_i = 0 else: raise if self.dof_list[0][-1] == 'e': sign = 1 elif self.dof_list[0][-1] == 'r': sign = -1 else: raise # Default command: command no movement on all valves # valves are yaw, pitch, knee, in that order # Scale depends on the mode of the underlying layer... # sometimes it expects a length rate in meters/sec, # sometimes it expects a command value between -255 and 255 cmd = [0.0, 0.0, 0.0] # Is this our first tick on a new DOF? if self.firstrun: # Initialize state variables print "Initializing for %s" % self.dof_list[0] self.pwm_val = 0 self.vel_IIR = LowPassFilter(gain=1.0, corner_frequency=.5) self.firstrun = False else: # Calculate velocity, update filter dt = time - self.last_t dpos = pos - self.last_pos vel_est = dpos / dt self.vel_IIR.update(vel_est) # Take 10 seconds to ramp valve command self.pwm_val += dt * 0.05 cmd[cmd_i] = sign * self.pwm_val self.last_t = time self.last_pos = pos # Are we moving faster than our threshold? if sign * self.vel_IIR.getVal() > self.MOVE_THRESH: # If so, note the PWM value we had to apply and prepare to move the # next joint self.file_out.write("%s: %f\n" % (self.dof_list[0], self.pwm_val)) print "%s: %f" % (self.dof_list[0], self.pwm_val) self.dof_list.pop(0) self.firstrun = True self.callback = self.pause return cmd
def discoverDeadband(self, time, yaw, hip_pitch, knee_pitch, shock_depth, command=None): # Have we made it through all the joints? if not len(self.dof_list): # FIXME: Exit safely self.file_out.close() self.callback = self.pause if self.dof_list[0][:-2] == 'hip_pitch': pos = hip_pitch cmd_i = 1 elif self.dof_list[0][:-2] == 'knee_pitch': pos = knee_pitch cmd_i = 2 elif self.dof_list[0][:-2] == 'hip_yaw': pos = yaw cmd_i = 0 else: raise if self.dof_list[0][-1] == 'e': sign = 1 elif self.dof_list[0][-1] == 'r': sign = -1 else: raise # Default command: command no movement on all valves # valves are yaw, pitch, knee, in that order # Scale depends on the mode of the underlying layer... # sometimes it expects a length rate in meters/sec, # sometimes it expects a command value between -255 and 255 cmd = [0.0, 0.0, 0.0] # Is this our first tick on a new DOF? if self.firstrun: # Initialize state variables print "Initializing for %s" % self.dof_list[0] self.pwm_val = 0 self.vel_IIR = LowPassFilter(gain=1.0, corner_frequency=.5) self.firstrun = False else: # Calculate velocity, update filter dt = time - self.last_t dpos = pos - self.last_pos vel_est = dpos / dt self.vel_IIR.update(vel_est) # Take 10 seconds to ramp valve command self.pwm_val += dt * 0.05 cmd[cmd_i] = sign * self.pwm_val self.last_t = time self.last_pos = pos # Are we moving faster than our threshold? if sign * self.vel_IIR.getVal() > self.MOVE_THRESH: # If so, note the PWM value we had to apply and prepare to move the # next joint self.file_out.write("%s: %f\n" % (self.dof_list[0], self.pwm_val)) print "%s: %f" % (self.dof_list[0], self.pwm_val) self.dof_list.pop(0) self.firstrun = True self.callback = self.pause return cmd
class JointController(object): """ Produces a linear rate trying to control an angle. Requires a description of a joint in the form of an ActuatorMathHelper """ def __init__(self, act_math, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10, backlash=0.0): """ act_math is the ActuatorMathHelper that contains the description of the joint and actuator kp = proportional gain term ki = integral gain term kd = derivative gain term kff= feed forward velocity term. Applies signal relative to dervative of the target derivative_corner = corner frequency of derivative filter. Our real world signals are too noisy to use without significant filtering kfa= feed forward acceleration term. Applies signal relative to the difference in rate of change of the target and desired. backlash = if we are within this angle of the target, we apply no corrective signal. """ self.act_math = act_math self.updateGainConstants(kp, ki, kd, kff, kfa) self.backlash = backlash # NOTE: it is important that these three variables are floating point to avoid truncation self.prev_error = 0.0 self.prev_desired_ang = 0.0 self.prev_ang = 0.0 self.integral_error_accumulator = 0.0 self.d_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner) self.arate_lowpass = LowPassFilter(gain=1.0, corner_frequency=derivative_corner) def updateGainConstants(self, kp, ki, kd, kff=None, kfa=None): self.kp = kp self.ki = ki self.kd = kd if kff != None: self.kff = kff else: self.kff = 0.0 if kfa != None: self.kfa = kfa else: self.kfa = 0.0 def getAngleRate(self): return self.arate_lowpass.getVal() def getLengthRate(self): self.act_math.setAngle(self.prev_ang) self.act_math.setAngleRate(self.arate_lowpass.getVal()) #self.act_math.setAngleRate( self.measured_deriv ) present_length_rate = self.act_math.getLengthRate() return present_length_rate def getDesiredLengthRate(self): self.act_math.setAngle(self.prev_ang) self.act_math.setAngleRate(self.desired_vel) desired_length_rate = self.act_math.getLengthRate() return desired_length_rate def update(self, desired_ang, measured_ang): delta_time = time_sources.global_time.getDelta() delta_ang = measured_ang - self.prev_ang self.prev_ang = measured_ang self.arate_lowpass.update(delta_ang / delta_time) desired_vel = (desired_ang - self.prev_desired_ang) / delta_time self.desired_vel = desired_vel error = desired_ang - measured_ang if abs(error) < self.backlash: error = 0.0 else: if error < 0.0: error += self.backlash else: error -= self.backlash self.integral_error_accumulator += self.ki * error * delta_time derivative_error = self.d_lowpass.update( (error - self.prev_error) / delta_time) velocity_error = desired_vel - derivative_error self.prev_error = error self.prev_desired_ang = desired_ang # Calculate a desired angle rate control_ang_rate = self.kp * error + self.integral_error_accumulator + self.kd * derivative_error + self.kff * desired_vel + self.kfa * velocity_error # Use joint geometry to determine a target piston length rate self.act_math.setAngle(measured_ang) self.act_math.setAngleRate(control_ang_rate) target_length_rate = self.act_math.getLengthRate() return target_length_rate
class JointController(object): """ Produces a linear rate trying to control an angle. Requires a description of a joint in the form of an ActuatorMathHelper """ def __init__(self, act_math, kp=0, ki=0, kd=0, kff=None, kfa=None, derivative_corner=10, backlash=0.0): """ act_math is the ActuatorMathHelper that contains the description of the joint and actuator kp = proportional gain term ki = integral gain term kd = derivative gain term kff= feed forward velocity term. Applies signal relative to dervative of the target derivative_corner = corner frequency of derivative filter. Our real world signals are too noisy to use without significant filtering kfa= feed forward acceleration term. Applies signal relative to the difference in rate of change of the target and desired. backlash = if we are within this angle of the target, we apply no corrective signal. """ self.act_math = act_math self.updateGainConstants(kp, ki, kd, kff, kfa) self.backlash = backlash # NOTE: it is important that these three variables are floating point to avoid truncation self.prev_error = 0.0 self.prev_desired_ang = 0.0 self.prev_ang = 0.0 self.integral_error_accumulator = 0.0 self.d_lowpass = LowPassFilter( gain=1.0, corner_frequency=derivative_corner ) self.arate_lowpass = LowPassFilter( gain=1.0, corner_frequency=derivative_corner ) def updateGainConstants(self, kp, ki, kd, kff = None, kfa=None): self.kp = kp self.ki = ki self.kd = kd if kff != None: self.kff = kff else: self.kff = 0.0 if kfa != None: self.kfa = kfa else: self.kfa = 0.0 def getAngleRate( self ): return self.arate_lowpass.getVal() def getLengthRate( self ): self.act_math.setAngle( self.prev_ang ) self.act_math.setAngleRate( self.arate_lowpass.getVal() ) #self.act_math.setAngleRate( self.measured_deriv ) present_length_rate = self.act_math.getLengthRate() return present_length_rate def getDesiredLengthRate( self ): self.act_math.setAngle( self.prev_ang ) self.act_math.setAngleRate( self.desired_vel ) desired_length_rate = self.act_math.getLengthRate() return desired_length_rate def update(self, desired_ang, measured_ang): delta_time = time_sources.global_time.getDelta() delta_ang = measured_ang - self.prev_ang self.prev_ang = measured_ang self.arate_lowpass.update( delta_ang / delta_time ) desired_vel = (desired_ang - self.prev_desired_ang)/delta_time self.desired_vel = desired_vel error = desired_ang - measured_ang if abs(error) < self.backlash: error = 0.0 else: if error < 0.0: error += self.backlash else: error -= self.backlash self.integral_error_accumulator += self.ki * error * delta_time derivative_error = self.d_lowpass.update((error - self.prev_error) / delta_time) velocity_error = desired_vel - derivative_error self.prev_error = error self.prev_desired_ang = desired_ang # Calculate a desired angle rate control_ang_rate = self.kp * error + self.integral_error_accumulator + self.kd * derivative_error + self.kff*desired_vel + self.kfa*velocity_error # Use joint geometry to determine a target piston length rate self.act_math.setAngle( measured_ang ) self.act_math.setAngleRate( control_ang_rate ) target_length_rate = self.act_math.getLengthRate() return target_length_rate
offset=45 * pi / 180, bias=(1200, 1200)) knee_encoder = EncoderNode(leg1, node_id=6, name="knee_encoder", gain=1, offset=225 * pi / 180, bias=(1200, 1200)) shock_encoder = EncoderNode(leg1, node_id=7, name="shock_encoder", gain=1, offset=0, bias=(1200, 1200)) pitch_filt = LowPassFilter(gain=1.0, corner_frequency=20) yaw_filt = LowPassFilter(gain=1.0, corner_frequency=20) # yaw_cmd_filt = LowPassFilter( gain=1.0, corner_frequency=100 ) publisher = Publisher(5055) publisher.addToCatalog('time', time.time) publisher.addToCatalog('yaw_ang', yaw_encoder.getAngleDeg) publisher.addToCatalog('yaw_ang_filt', lambda: 180 * yaw_filt.getVal() / pi) publisher.addToCatalog('yaw_sin', yaw_encoder.getSinValue) publisher.addToCatalog('yaw_cos', yaw_encoder.getCosValue) publisher.addToCatalog('pitch_ang', pitch_encoder.getAngleDeg) publisher.addToCatalog('pitch_ang_filt', lambda: 180 * pitch_filt.getVal() / pi) publisher.addToCatalog('pitch_sin', pitch_encoder.getSinValue) publisher.addToCatalog('pitch_cos', pitch_encoder.getCosValue) publisher.addToCatalog('knee_ang', knee_encoder.getAngleDeg)