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.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)
