def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') if rospy.has_param(self.controller_namespace + '/motor/acceleration'): self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration') else: self.acceleration = None if rospy.has_param(self.controller_namespace + '/motor/pid'): self.pid_gains = rospy.get_param(self.controller_namespace + '/motor/pid') else: self.pid_gains = [] if rospy.has_param('cute_servo_version'): self.servo_version = rospy.get_param('cute_servo_version') else: self.servo_version = 'xqtor_1' self.flipped = self.min_angle_raw > self.max_angle_raw self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') self.min_angle_treshold = rospy.get_param(self.controller_namespace + '/motor/minAngle') self.max_angle_treshold = rospy.get_param(self.controller_namespace + '/motor/maxAngle') if rospy.has_param(self.controller_namespace + '/motor/acceleration'): self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration') else: self.acceleration = None self.flipped = self.min_angle_raw > self.max_angle_raw self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') self.flipped = self.min_angle_raw > self.max_angle_raw self.last_commanded_torque = 0.0 self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id]) self.arm_state = JointState(name=self.joint_name, motor_ids=[self.motor_id]) self.joint_state_out = JointStateOut() self.joint_state_out.name = [] self.joint_state_out.position = [] self.joint_state_out.velocity = [] self.joint_state_out.effort = [] self.wrench_state = WrenchStamped() self.wrench_state.header.frame_id = "" self.wrench_state.wrench.force.x = 0 self.wrench_state.wrench.force.y = 0 self.wrench_state.wrench.force.z = 0 self.wrench_state.wrench.torque.x = 0 self.wrench_state.wrench.torque.y = 0 self.wrench_state.wrench.torque.z = 0
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.msg = 0 # in order to keep the torque up and running we need to save this value. self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') self.min_angle_treshold = rospy.get_param(self.controller_namespace + '/motor/minAngle') self.max_angle_treshold = rospy.get_param(self.controller_namespace + '/motor/maxAngle') if rospy.has_param(self.controller_namespace + '/motor/acceleration'): self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration') else: self.acceleration = None self.flipped = self.min_angle_raw > self.max_angle_raw self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') if rospy.has_param(self.controller_namespace + '/motor/acceleration'): self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration') else: self.acceleration = None # added motor bias [AHC: 20160825] if rospy.has_param(self.controller_namespace + '/motor/bias'): self.motor_bias = rospy.get_param(self.controller_namespace + '/motor/bias') else: self.motor_bias = 0.0 rospy.loginfo( "JointPositionController::JointPositionController() - Motor %d bias set to %.4f\n", self.motor_id, self.motor_bias) self.flipped = self.min_angle_raw > self.max_angle_raw self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id') self.master_initial_position_raw = rospy.get_param( self.controller_namespace + '/motor_master/init') self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min') self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max') self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id') self.slave_offset = rospy.get_param( self.controller_namespace + '/motor_slave/calibration_offset', 0) if (self.slave_id == 9 ): #Using unchanged normal operations for gripper motors self.normal_slave = True else: self.normal_slave = False self.flipped = self.master_min_angle_raw > self.master_max_angle_raw self.joint_state = JointState( name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') if rospy.has_param(self.controller_namespace + '/motor/acceleration'): self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration') else: self.acceleration = None # added motor compliance slope and compliance margin [RX:02142019] if rospy.has_param(self.controller_namespace + '/joint_compliance_slope'): self.compliance_slope = rospy.get_param(self.controller_namespace + '/joint_compliance_slope', None) if rospy.has_param(self.controller_namespace + '/joint_compliance_margin'): self.compliance_margin = rospy.get_param(self.controller_namespace + '/joint_compliance_margin', None) # added motor pid parameters [RX:10082018] self.p_gain = None self.i_gain = None self.d_gain = None if rospy.has_param(self.controller_namespace + '/motor/p_gain'): self.p_gain = rospy.get_param(self.controller_namespace + '/motor/p_gain', None) if rospy.has_param(self.controller_namespace + '/motor/i_gain'): self.i_gain = rospy.get_param(self.controller_namespace + '/motor/i_gain', None) if rospy.has_param(self.controller_namespace + '/motor/d_gain'): self.d_gain = rospy.get_param(self.controller_namespace + '/motor/d_gain', None) # added motor bias [AHC: 20160825] if rospy.has_param(self.controller_namespace + '/motor/bias'): self.motor_bias = rospy.get_param(self.controller_namespace + '/motor/bias') else: self.motor_bias = 0.0 # added motor torque limit if rospy.has_param(self.controller_namespace + '/joint_torque_limit'): self.torque_limit = rospy.get_param(self.controller_namespace + '/joint_torque_limit') else: self.torque_limit = 1.0 # added joint torque enabled [RX:02142019] if rospy.has_param(self.controller_namespace + '/joint_torque_enabled'): self.torque_enabled = rospy.get_param(self.controller_namespace + '/joint_torque_enabled') else: self.torque_enabled = True rospy.loginfo("JointPositionController::JointPositionController() - Motor %d bias set to %.4f\n", self.motor_id, self.motor_bias) if self.compliance_slope is not None: rospy.loginfo("JointPositionController::JointPositionController() - Motor %d Compliance slope set to %d\n", self.motor_id, self.compliance_slope) if self.compliance_margin is not None: rospy.loginfo("JointPositionController::JointPositionController() - Motor %d Compliance margin set to %d\n", self.motor_id, self.compliance_margin) self.flipped = self.min_angle_raw > self.max_angle_raw self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') self.flipped = self.min_angle_raw > self.max_angle_raw self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id') self.master_initial_position_raw = rospy.get_param(self.controller_namespace + '/motor_master/init') self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min') self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max') self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id') self.slave_offset = rospy.get_param(self.controller_namespace + '/motor_slave/calibration_offset', 0) self.flipped = self.master_min_angle_raw > self.master_max_angle_raw self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id') self.master_initial_position_raw = rospy.get_param(self.controller_namespace + '/motor_master/init') self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min') self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max') self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id') self.flipped = self.master_min_angle_raw > self.master_max_angle_raw self.last_commanded_torque = 0.0 self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init') self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min') self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max') if rospy.has_param(self.controller_namespace + '/motor/acceleration'): self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration') else: self.acceleration = None if rospy.has_param(self.controller_namespace + '/write_limits_to_servo'): self.write_limits_to_servo = rospy.get_param(self.controller_namespace + '/write_limits_to_servo') else: self.write_limits_to_servo = False self.flipped = self.min_angle_raw > self.max_angle_raw self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id])
def __init__(self, dxl_io, controller_namespace, port_namespace): JointController.__init__(self, dxl_io, controller_namespace, port_namespace) self.master_id = rospy.get_param(self.controller_namespace + '/motor_master/id') self.master_initial_position_raw = rospy.get_param( self.controller_namespace + '/motor_master/init') self.master_min_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/min') self.master_max_angle_raw = rospy.get_param(self.controller_namespace + '/motor_master/max') self.slave_id = rospy.get_param(self.controller_namespace + '/motor_slave/id') self.slave_offset = rospy.get_param( self.controller_namespace + '/motor_slave/calibration_offset', 0) self.flipped = self.master_min_angle_raw > self.master_max_angle_raw self.joint_state = JointDualState( name=self.joint_name, motor_ids=[self.master_id, self.slave_id]) self.joint_state_publisher = rospy.Publisher( self.controller_namespace + '/state', JointDualState)