implementation proposal for "Set Gains"

[manuelnt11]

I have been working with dynamixel motors and I needed set gains like parameters in the tilt.yaml. Well, I implemented this function and may be you want to do a clear implementation in the module.

This is my changes in the python code -> joint_controller.py <-

  self.compliance_slope = rospy.get_param(self.controller_namespace + '/joint_compliance_slope', None)
+ self.p_gain = rospy.get_param(self.controller_namespace + '/joint_p_gain', None)
+ self.i_gain = rospy.get_param(self.controller_namespace + '/joint_i_gain', None)
+ self.d_gain = rospy.get_param(self.controller_namespace + '/joint_d_gain', None)

  self.compliance_slope_service = rospy.Service(self.controller_namespace + '/set_compliance_slope', SetComplianceSlope, self.process_set_compliance_slope)
+ self.p_gain_service = rospy.Service(self.controller_namespace + '/set_p_gain', SetComplianceSlope, self.process_set_p_gain)
+ self.i_gain_service = rospy.Service(self.controller_namespace + '/set_i_gain', SetComplianceSlope, self.process_set_i_gain)
+ self.d_gain_service = rospy.Service(self.controller_namespace + '/set_d_gain', SetComplianceSlope, self.process_set_d_gain)

  if self.compliance_slope is not None:
      if self.compliance_slope < DXL_MIN_COMPLIANCE_SLOPE: self.compliance_slope = DXL_MIN_COMPLIANCE_SLOPE
      elif self.compliance_slope > DXL_MAX_COMPLIANCE_SLOPE: self.compliance_slope = DXL_MAX_COMPLIANCE_SLOPE
      else: self.compliance_slope = int(self.compliance_slope)

+ if self.p_gain is not None:
+     if self.p_gain > 254: self.p_gain = 254
+     else: self.p_gain = int(self.p_gain)
+
+ if self.i_gain is not None:
+     if self.i_gain > 254: self.i_gain = 254
+     else: self.i_gain = int(self.i_gain)
+     
+ if self.d_gain is not None:
+     if self.d_gain > 254: self.d_gain = 254
+     else: self.d_gain = int(self.d_gain)

  def set_compliance_slope(self, slope):
      raise NotImplementedError

+ def set_p_gain(self, gain):
+     raise NotImplementedError
+
+ def set_i_gain(self, gain):
+     raise NotImplementedError
+
+ def set_d_gain(self, gain):
+     raise NotImplementedError

  def process_set_compliance_slope(self, req):
      self.set_compliance_slope(req.slope)
      return []

+ def process_set_p_gain(self, req):
+     self.set_p_gain(req.slope)
+     return []
+ 
+ def process_set_i_gain(self, req):
+     self.set_i_gain(req.slope)
+     return []
+ 
+ def process_set_d_gain(self, req):
+     self.set_d_gain(req.slope)
+     return []

-> joint_position_controller.py <-

  if self.compliance_slope is not None: self.set_compliance_slope(self.compliance_slope)
+ if self.p_gain is not None: self.set_p_gain(self.p_gain)
+ if self.i_gain is not None: self.set_i_gain(self.i_gain)
+ if self.d_gain is not None: self.set_d_gain(self.d_gain)

  def set_compliance_slope(self, slope):
      if slope < DXL_MIN_COMPLIANCE_SLOPE: slope = DXL_MIN_COMPLIANCE_SLOPE
      elif slope > DXL_MAX_COMPLIANCE_SLOPE: slope = DXL_MAX_COMPLIANCE_SLOPE
      mcv = (self.motor_id, slope, slope)
      self.dxl_io.set_multi_compliance_slopes([mcv])

+ def set_p_gain(self, gain):
+     if gain > 254: gain = 254
+     self.dxl_io.set_p_gain(self.motor_id, gain)
+
+ def set_i_gain(self, gain):
+     if gain > 254: gain = 254
+     self.dxl_io.set_i_gain(self.motor_id, gain)
+
+ def set_d_gain(self, gain):
+     if gain > 254: gain = 254
+     self.dxl_io.set_d_gain(self.motor_id, gain)

[manuelnt11]

-> dynamixel_io.py <-

  def set_d_gain(self, servo_id, d_gain):
      """
      Sets the value of derivative action of PID controller.
      Gain value is in range 0 to 254.
      """
      response = self.write(servo_id, DXL_D_GAIN, [d_gain])
      if response:
-         self.exception_on_error(response[4], servo_id, 'setting D gain value of PID controller to %d' % slope)
+         self.exception_on_error(response[4], servo_id, 'setting D gain value of PID controller to %d' % d_gain)
      return response

  def set_i_gain(self, servo_id, i_gain):
      """
      Sets the value of integral action of PID controller.
      Gain value is in range 0 to 254.
      """
      response = self.write(servo_id, DXL_I_GAIN, [i_gain])
      if response:
-         self.exception_on_error(response[4], servo_id, 'setting I gain value of PID controller to %d' % slope)
+         self.exception_on_error(response[4], servo_id, 'setting I gain value of PID controller to %d' % i_gain)
      return response

  def set_p_gain(self, servo_id, p_gain):
      """
      Sets the value of proportional action of PID controller.
      Gain value is in range 0 to 254.
      """
      response = self.write(servo_id, DXL_P_GAIN, [p_gain])
      if response:
-         self.exception_on_error(response[4], servo_id, 'setting P gain value of PID controller to %d' % slope)
+         self.exception_on_error(response[4], servo_id, 'setting P gain value of PID controller to %d' % p_gain)
      return response