def __init__(self, name, hand_or_arm="both"):
self.__save = rospy.ServiceProxy(
'save_robot_state', SaveState)
self.__name = name
if hand_or_arm == "arm":
self.__commander = SrArmCommander()
elif hand_or_arm == 'hand':
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
self.__commander = SrHandCommander(hand_parameters=hand_parameters,
hand_serial=hand_serial)
else:
self.__arm_commander = SrArmCommander()
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
self.__hand_commander = SrHandCommander(hand_parameters=hand_parameters,
hand_serial=hand_serial)
self.__hand_or_arm = hand_or_arm
def test_one_hand_no_mapping_no_robot_description_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "")
hand_finder = HandFinder()
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(),
"Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1,
"It should be only one mapping")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
1, "It should be only one joint_prefix")
self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1",
"It should be the serial id as mapping")
self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'],
"rh_", "It should be only rh_ prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(list(hand_finder.get_hand_joints().keys())), 1,
"It should be only one mapping")
print(hand_finder.get_hand_joints())
self.assertIn("1", hand_finder.get_hand_joints(),
"Serial should be in the joints result")
joints = hand_finder.get_hand_joints()['1'] # use serial
self.assertEqual(len(joints), 24, "Joint number should be 24")
self.assertIn("rh_FFJ3",
hand_finder.get_hand_joints()["1"],
"FFJ3 joint should be in the joints list")
def test_no_hand_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("robot_description",
rospy.get_param("two_hands_description"))
hand_finder = HandFinder()
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
0, "correct parameters without a hand")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0,
"correct parameters without a hand")
self.assertEqual(len(hand_finder.get_hand_joints()), 0,
"correct joints without a hand")
self.assertEqual(len(hand_finder.get_calibration_path()), 0,
"correct calibration path without a hand")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().friction_compensation),
0, "correct tuning without a hands")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().host_control), 0,
"correct tuning without a hands")
self.assertEqual(
len(hand_finder.get_hand_control_tuning().motor_control), 0,
"correct tuning without a hands")
def __init__(self, name, hand_or_arm="both", hand_h=False):
self.__save = rospy.ServiceProxy('save_robot_state', SaveState)
self.__name = name
self.__hand_h = hand_h
if hand_or_arm == "arm":
self.__commander = SrArmCommander()
elif hand_or_arm == 'hand':
if self.__hand_h:
self.__commander = SrRobotCommander("hand_h", prefix="H1_")
else:
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
self.__commander = SrHandCommander(
hand_parameters=hand_parameters, hand_serial=hand_serial)
else:
self.__arm_commander = SrArmCommander()
if self.__hand_h:
self.__hand_commander = SrRobotCommander("hand_h",
prefix="H1_")
else:
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
self.__hand_commander = SrHandCommander(
hand_parameters=hand_parameters, hand_serial=hand_serial)
self.__hand_or_arm = hand_or_arm
rs = RobotState()
current_dict = {}
if self.__hand_or_arm == "both":
current_dict = self.__arm_commander.get_robot_state_bounded()
robot_name = self.__arm_commander.get_robot_name()
elif self.__hand_or_arm == "arm":
current_dict = self.__commander.get_current_state_bounded()
robot_name = self.__commander.get_robot_name()
elif self.__hand_or_arm == "hand":
current_dict = self.__commander.get_current_state_bounded()
robot_name = self.__commander.get_robot_name()
else:
rospy.logfatal("Unknown save type")
exit(-1)
rospy.loginfo(current_dict)
rs.joint_state = JointState()
rs.joint_state.name = current_dict.keys()
rs.joint_state.position = current_dict.values()
self.__save(self.__name, robot_name, rs)
def test_no_hand_no_robot_description_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
hand_finder = HandFinder()
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
0, "correct parameters without a hand")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 0,
"correct parameters without a hand")
self.assertEqual(len(hand_finder.get_hand_joints()), 0,
"correct joints without a hand")
def test_hand_finder_init(self):
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_commander = SrHandCommander(
hand_parameters=hand_parameters,
hand_serial=hand_parameters.mapping.keys()[0])
self.assertGreater(len(hand_commander.get_joints_position()), 0,
"No joints found, init must have failed.")
def test_two_hand_no_robot_description_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "right")
rospy.set_param("hand/joint_prefix/2", "lh_")
rospy.set_param("hand/mapping/2", "left")
hand_finder = HandFinder()
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(),
"Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 2,
"It should be two mappings")
self.assertIn("right",
list(hand_finder.get_hand_parameters().mapping.values()),
"It should be right mapping")
self.assertIn("left",
list(hand_finder.get_hand_parameters().mapping.values()),
"It should be left mapping")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
2, "It should be two joint_prefixes")
self.assertIn(
"rh_",
list(hand_finder.get_hand_parameters().joint_prefix.values()),
"It should be rh_ prefix")
self.assertIn(
"lh_",
list(hand_finder.get_hand_parameters().joint_prefix.values()),
"It should be rh_ prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(list(hand_finder.get_hand_joints().keys())), 2,
"It should be two mappings")
self.assertIn("right", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['right']
self.assertEqual(len(joints), 24, "Joint number should be 24")
self.assertIn("rh_FFJ3",
hand_finder.get_hand_joints()["right"],
"FFJ3 joint should be in the joints list")
self.assertIn("left", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['left']
self.assertEqual(len(joints), 24, "Joint number should be 24")
self.assertIn("lh_FFJ1",
hand_finder.get_hand_joints()["left"],
"FFJ1 joint should be in the joints list")
def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "")
rospy.set_param("hand/mapping/1", "")
rospy.set_param("robot_description",
rospy.get_param("right_hand_description_no_prefix"))
hand_finder = HandFinder()
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(),
"Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1,
"It should be only one mapping")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
1, "It should be only one joint_prefix")
self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1",
"It should be the serial id as mapping")
self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'],
"", "It should be only an empty prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(list(hand_finder.get_hand_joints().keys())), 1,
"It should be only one mapping")
self.assertIn("1", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['1']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("FFJ3",
hand_finder.get_hand_joints()["1"],
"FFJ3 joint should not be in the joints list")
self.assertIn("RFJ4",
hand_finder.get_hand_joints()["1"],
"RFJ4 joint should be in the joints list")
def __init__(self, config_file_path, service_timeout, excluded_joints=[]):
self._config_file_path = config_file_path
self._service_timeout = service_timeout
hand_finder = HandFinder()
self._hand_mapping = hand_finder.get_hand_parameters().mapping
self._joints = hand_finder.get_hand_joints()
self._nonpresent_joints = list(ControllerSpawner.JOINT_NAMES)
for side in self._joints:
for joint in self._joints[side]:
if joint in self._nonpresent_joints:
self._nonpresent_joints.remove(joint)
self._excluded_joints = excluded_joints
self._excluded_joints = list(
set(self._excluded_joints) | set(self._nonpresent_joints))
rospy.logwarn("Excluded joints:")
rospy.logwarn(self._excluded_joints)
def __init__(self, name, prefix=None):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy("get_robot_state",
GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self.__plan = None
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
# prefix of the trajectory controller
if prefix is not None:
self._prefix = prefix
elif name in self.__group_prefixes.keys():
self._prefix = self.__group_prefixes[name]
else:
# Group name is one of the ones to plan for specific fingers.
# We need to find the hand prefix using the hand finder
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
self._prefix = hand_parameters.joint_prefix[hand_serial]
self._set_up_action_client()
threading.Thread(None, rospy.spin)
def generate_controllers():
controllers = []
hand_finder = HandFinder()
joints = hand_finder.get_hand_joints()
mapping = hand_finder.get_hand_parameters().mapping
for hand in mapping:
prefix = mapping[hand]
for joint in joints[prefix]:
if joint[3:5].lower() == 'th' or joint[3:5].lower(
) == 'wr' or (joint[6] != '1' and joint[6] != '2'):
joint_controller = 'cal_sh_' + joint.lower()
else:
joint = joint[:6] + '0'
joint_controller = 'cal_sh_' + joint.lower()
if joint_controller not in controllers:
controllers.append(joint_controller)
return controllers
def generate_reset_services_list(self):
reset_service_list = []
generated_reset_service_list = []
service_list = []
# We first read the list of available motor reset services in this namespace
# this will allow us to avoid having to know the name of the robot driver node
ns = rospy.get_namespace()
while not reset_service_list:
rospy.sleep(0.5)
service_list = rosservice.get_service_list(namespace=ns)
reset_service_list = [
srv for srv in service_list if '/reset_motor_' in srv
]
if not reset_service_list:
rospy.loginfo("Waiting for motor reset services")
# We generate a list of the services that we expect to find
# and merely throw a warning if some of them didn't turn up in the available service list
hand_finder = HandFinder()
joints = hand_finder.get_hand_joints()
mapping = hand_finder.get_hand_parameters().mapping
for hand in mapping:
prefix = mapping[hand]
for joint in joints[prefix]:
if not (joint[3:5].lower() == 'th'
or joint[3:5].lower() == 'wr' or
(joint[6] != '1' and joint[6] != '2')):
joint = joint[:6] + '0'
joint_reset_service = joint[:2] + '/reset_motor_' + joint[
3:].upper()
if joint_reset_service not in generated_reset_service_list:
generated_reset_service_list.append(joint_reset_service)
for gen_srv in generated_reset_service_list:
gen_srv_match_list = [
srv for srv in reset_service_list if gen_srv in srv
]
if not gen_srv_match_list:
rospy.logwarn("Expected service not found: %s", gen_srv)
return reset_service_list
from sr_utilities.hand_finder import HandFinder
rospy.init_node("partial_traj_example", anonymous=True)
rospy.sleep(1) # Do not start at time zero
def construct_trajectory_point(posture, duration):
trajectory_point = JointTrajectoryPoint()
trajectory_point.time_from_start = rospy.Duration.from_sec(float(duration))
for key in joint_trajectory.joint_names:
trajectory_point.positions.append(posture[key])
return trajectory_point
hand_finder = HandFinder()
hand_parameters = hand_finder.get_hand_parameters()
hand_serial = hand_parameters.mapping.keys()[0]
hand_commander = SrHandCommander(hand_parameters=hand_parameters,
hand_serial=hand_serial)
hand_mapping = hand_parameters.mapping[hand_serial]
# Hand joints are detected
joints = hand_finder.get_hand_joints()[hand_mapping]
open_hand = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 0.0, 'rh_FFJ4': 0.0,
'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 0.0, 'rh_MFJ4': 0.0,
'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0,
'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0,
'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0,
def test_two_hand_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "rh_")
rospy.set_param("hand/mapping/1", "right")
rospy.set_param("hand/joint_prefix/2", "lh_")
rospy.set_param("hand/mapping/2", "left")
rospy.set_param("robot_description",
rospy.get_param("two_hands_description"))
hand_finder = HandFinder()
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(),
"Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 2,
"It should be two mappings")
self.assertIn("right",
hand_finder.get_hand_parameters().mapping.values(),
"It should be right mapping")
self.assertIn("left",
hand_finder.get_hand_parameters().mapping.values(),
"It should be left mapping")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
2, "It should be two joint_prefixes")
self.assertIn("rh_",
hand_finder.get_hand_parameters().joint_prefix.values(),
"It should be rh_ prefix")
self.assertIn("lh_",
hand_finder.get_hand_parameters().joint_prefix.values(),
"It should be rh_ prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(hand_finder.get_hand_joints().keys()), 2,
"It should be two mappings")
self.assertIn("right", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['right']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("rh_FFJ3",
hand_finder.get_hand_joints()["right"],
"rh_FFJ3 joint should not be in the joints list")
self.assertIn("rh_RFJ4",
hand_finder.get_hand_joints()["right"],
"rh_RFJ4 joint should be in the joints list")
self.assertIn("left", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['left']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("lh_FFJ3",
hand_finder.get_hand_joints()["left"],
"lh_FFJ3 joint should not be in the joints list")
self.assertIn("lh_LFJ4",
hand_finder.get_hand_joints()["left"],
"lh_LFJ4 joint should be in the joints list")
# calibration
self.assertIsNotNone(hand_finder.get_calibration_path(),
"Calibration extracted.")
calibration_path = hand_finder.get_calibration_path()['right']
self.assertEqual(
calibration_path,
self.ethercat_path + "/calibrations/right/" + "calibration.yaml",
"incorrect calibration file")
calibration_path = hand_finder.get_calibration_path()['left']
self.assertEqual(
calibration_path,
self.ethercat_path + "/calibrations/left/" + "calibration.yaml",
"incorrect calibration file")
# tuning
self.assertIsNotNone(hand_finder.get_hand_control_tuning(),
"Control tuning parameters extracted.")
ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning(
).friction_compensation['right']
self.assertEqual(
ctrl_tun_friction_comp_path,
self.ethercat_path + "/controls/friction_compensation.yaml",
"incorrect friction compensation file")
ctrl_tun_motors_path = hand_finder.get_hand_control_tuning(
).motor_control['right']
self.assertEqual(
ctrl_tun_motors_path, self.ethercat_path +
"/controls/motors/right/motor_board_effort_controllers.yaml",
"incorrect motor config file")
ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning(
).host_control['right']
self.assertEqual(len(ctrl_tun_host_control_paths),
len(controller_params),
"incorrect number of host controllers param")
for controller_path, controller_param in zip(
ctrl_tun_host_control_paths, controller_params):
self.assertEqual(
controller_path, self.ethercat_path + "/controls/host/right/" +
controller_param, "incorrect controller config file")
ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning(
).friction_compensation['left']
self.assertEqual(
ctrl_tun_friction_comp_path,
self.ethercat_path + "/controls/friction_compensation.yaml",
"incorrect friction compensation file")
ctrl_tun_motors_path = hand_finder.get_hand_control_tuning(
).motor_control['left']
self.assertEqual(
ctrl_tun_motors_path, self.ethercat_path +
"/controls/motors/left/motor_board_effort_controllers.yaml",
"incorrect motor config file")
ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning(
).host_control['left']
self.assertEqual(len(ctrl_tun_host_control_paths),
len(controller_params),
"incorrect number of host controllers param")
for controller_path, controller_param in zip(
ctrl_tun_host_control_paths, controller_params):
self.assertEqual(
controller_path,
self.ethercat_path + "/controls/host/left/" + controller_param,
"incorrect controller config file")
def test_one_hand_no_prefix_no_ns_robot_description_exists_finder(self):
if rospy.has_param("hand"):
rospy.delete_param("hand")
if rospy.has_param("robot_description"):
rospy.delete_param("robot_description")
rospy.set_param("hand/joint_prefix/1", "")
rospy.set_param("hand/mapping/1", "")
rospy.set_param("robot_description",
rospy.get_param("right_hand_description_no_prefix"))
hand_finder = HandFinder()
# hand params
self.assertIsNotNone(hand_finder.get_hand_parameters(),
"Parameters extracted.")
self.assertEqual(len(hand_finder.get_hand_parameters().mapping), 1,
"It should be only one mapping")
self.assertEqual(len(hand_finder.get_hand_parameters().joint_prefix),
1, "It should be only one joint_prefix")
self.assertEqual(hand_finder.get_hand_parameters().mapping['1'], "1",
"It should be the serial id as mapping")
self.assertEqual(hand_finder.get_hand_parameters().joint_prefix['1'],
"", "It should be only an empty prefix")
# hand joints
self.assertIsNotNone(hand_finder.get_hand_joints(),
"Joints extracted.")
self.assertEqual(len(hand_finder.get_hand_joints().keys()), 1,
"It should be only one mapping")
self.assertIn("1", hand_finder.get_hand_joints(),
"Mapping should be in the joints result")
joints = hand_finder.get_hand_joints()['1']
self.assertEqual(len(joints), 1, "Joint number should be 1")
self.assertNotIn("FFJ3",
hand_finder.get_hand_joints()["1"],
"FFJ3 joint should not be in the joints list")
self.assertIn("RFJ4",
hand_finder.get_hand_joints()["1"],
"RFJ4 joint should be in the joints list")
# calibration
self.assertIsNotNone(hand_finder.get_calibration_path(),
"Calibration extracted.")
calibration_path = hand_finder.get_calibration_path()['1']
self.assertEqual(
calibration_path,
self.ethercat_path + "/calibrations/1/" + "calibration.yaml",
"incorrect calibration file")
# tuning
self.assertIsNotNone(hand_finder.get_hand_control_tuning(),
"Control tuning parameters extracted.")
ctrl_tun_friction_comp_path = hand_finder.get_hand_control_tuning(
).friction_compensation['1']
self.assertEqual(
ctrl_tun_friction_comp_path,
self.ethercat_path + "/controls/friction_compensation.yaml",
"incorrect friction compensation file")
ctrl_tun_motors_path = hand_finder.get_hand_control_tuning(
).motor_control['1']
self.assertEqual(
ctrl_tun_motors_path, self.ethercat_path +
"/controls/motors/1/motor_board_effort_controllers.yaml",
"incorrect motor config file")
ctrl_tun_host_control_paths = hand_finder.get_hand_control_tuning(
).host_control['1']
self.assertEqual(len(ctrl_tun_host_control_paths),
len(controller_params),
"incorrect number of host controllers param")
for controller_path, controller_param in zip(
ctrl_tun_host_control_paths, controller_params):
self.assertEqual(
controller_path,
self.ethercat_path + "/controls/host/1/" + controller_param,
"incorrect controller config file")
class EtherCAT_Hand_Lib(object):
"""
Useful python library to communicate with the etherCAT hand.
"""
sensors = ["FFJ1", "FFJ2", "FFJ3", "FFJ4",
"MFJ1", "MFJ2", "MFJ3", "MFJ4",
"RFJ1", "RFJ2", "RFJ3", "RFJ4",
"LFJ1", "LFJ2", "LFJ3", "LFJ4", "LFJ5",
"THJ1", "THJ2", "THJ3", "THJ4", "THJ5A", "THJ5B",
"WRJ1A", "WRJ1B", "WRJ2",
"ACCX", "ACCY", "ACCZ",
"GYRX", "GYRY", "GYRZ",
"AN0", "AN1", "AN2", "AN3"]
def __init__(self):
"""
Useful python library to communicate with the etherCAT hand.
"""
self.hand_finder = HandFinder()
self.hand_params = self.hand_finder.get_hand_parameters()
self.hand_id = ''
if len(self.hand_params.mapping) is not 0:
self.hand_id = self.hand_params.mapping.itervalues().next()
self.debug_subscriber = None
self.joint_state_subscriber = None
self.record_js_callback = None
self.raw_values = []
self.pid_services = {}
self.publishers = {}
self.positions = {}
self.velocities = {}
self.efforts = {}
self.msg_to_send = Float64()
self.msg_to_send.data = 0.0
# TODO: read this from parameter server
self.compounds = {}
joint_to_sensor_mapping = []
try:
rospy.wait_for_message(self.hand_id + "/debug_etherCAT_data",
EthercatDebug, timeout=0.2)
try:
joint_to_sensor_mapping \
= rospy.get_param(self.hand_id + "/joint_to_sensor_mapping")
except:
rospy.logwarn("The parameter joint_to_sensor_mapping "
"was not found, you won't be able to get the "
"raw values from the EtherCAT compound sensors.")
except:
pass
for mapping in joint_to_sensor_mapping:
if mapping[0] is 1:
if "THJ5" in mapping[1][0]:
self.compounds["THJ5"] = [["THJ5A", mapping[1][1]],
["THJ5B", mapping[2][1]]]
elif "WRJ1" in mapping[1][0]:
self.compounds["WRJ1"] = [["WRJ1A", mapping[1][1]],
["WRJ1B", mapping[2][1]]]
def sendupdate(self, joint_name, target, controller_type="effort"):
if joint_name not in self.publishers:
topic = "sh_" + joint_name.lower() + "_" + controller_type \
+ "_controller/command"
self.publishers[joint_name] = rospy.Publisher(topic, Float64)
self.msg_to_send.data = math.radians(float(target))
self.publishers[joint_name].publish(self.msg_to_send)
def get_position(self, joint_name):
value = None
try:
value = self.positions[joint_name]
except:
# We check if the reason to except is that we are trying to
# access the joint 0
# Position of the J0 is the addition of the positions of J1 and J2
m = re.match("(?P<finger>\w{2})J0", joint_name)
if m is not None:
value = self.positions[m.group("finger") + "J1"] +\
self.positions[m.group("finger") + "J2"]
else:
raise
return value
def get_velocity(self, joint_name):
value = None
try:
value = self.velocities[joint_name]
except:
# We check if the reason to except is that we are
# trying to access the joint 0
m = re.match("(?P<finger>\w{2})J0", joint_name)
if m is not None:
value = self.velocities[m.group("finger") + "J1"] + \
self.velocities[m.group("finger") + "J2"]
else:
raise
return value
def get_effort(self, joint_name):
value = None
try:
value = self.efforts[joint_name]
except:
# We check if the reason to except is that we are
# trying to access the joint 0
# Effort of the J0 is the same as the effort of
# J1 and J2, so we pick J1
m = re.match("(?P<finger>\w{2})J0", joint_name)
if m is not None:
value = self.efforts[m.group("finger") + "J1"]
else:
raise
return value
def start_record(self, joint_name, callback):
self.record_js_callback = callback
def stop_record(self, joint_name):
self.callback = None
def set_pid(self, joint_name, pid_parameters):
"""
"""
if joint_name not in self.pid_services:
service_name = "sr_hand_robot/change_force_PID_" + joint_name
self.pid_services[joint_name] = rospy.ServiceProxy(service_name,
ForceController)
self.pid_services[joint_name](pid_parameters["max_pwm"],
pid_parameters["sgleftref"],
pid_parameters["sgrightref"],
pid_parameters["f"],
pid_parameters["p"],
pid_parameters["i"],
pid_parameters["d"],
pid_parameters["imax"],
pid_parameters["deadband"],
pid_parameters["sign"])
def debug_callback(self, msg):
if not(all(v == 0 for v in msg.sensors)):
self.raw_values = msg.sensors
def joint_state_callback(self, msg):
for name, pos, vel, effort in \
zip(msg.name, msg.position, msg.velocity, msg.effort):
self.positions[name] = pos
self.velocities[name] = vel
self.efforts[name] = effort
if self.record_js_callback is not None:
self.record_js_callback()
def get_raw_value(self, sensor_name):
value = 0.0
if not self.raw_values:
rospy.logwarn("No values received from the etherCAT hand.")
return -1.0
try:
if sensor_name in self.compounds.keys():
for sub_compound in self.compounds[sensor_name]:
index = self.sensors.index(sub_compound[0])
value = value + (self.raw_values[index] * sub_compound[1])
else:
index = self.sensors.index(sensor_name)
value = self.raw_values[index]
except:
# if the value is not found we're returning 4095
value = 4095
return value
def get_average_raw_value(self, sensor_name, number_of_samples=10):
"""
Get the average raw value for the given sensor, average on
number_of_samples
"""
tmp_raw_values = []
for i in range(0, number_of_samples):
tmp_raw_values.append(self.get_raw_value(sensor_name))
time.sleep(0.002)
average = float(sum(tmp_raw_values)) / len(tmp_raw_values)
return average
def activate(self):
# check if something is being published to those topics, otherwise
# return false (the library is not activated)
try:
rospy.wait_for_message(self.hand_id + "/debug_etherCAT_data",
EthercatDebug, timeout=0.2)
rospy.wait_for_message("joint_states", JointState, timeout=0.2)
except:
return False
self.debug_subscriber =\
rospy.Subscriber(self.hand_id + "/debug_etherCAT_data",
EthercatDebug, self.debug_callback)
self.joint_state_subscriber =\
rospy.Subscriber("joint_states",
JointState, self.joint_state_callback)
return True
def activate_joint_states(self):
# check if something is being published to this topic, otherwise
# return false (the library is not activated)
try:
rospy.wait_for_message("joint_states", JointState, timeout=0.2)
self.joint_state_subscriber = \
rospy.Subscriber("joint_states", JointState,
self.joint_state_callback)
except:
return False
return True
def on_close(self):
if self.debug_subscriber is not None:
self.debug_subscriber.unregister()
self.debug_subscriber = None
if self.joint_state_subscriber is not None:
self.joint_state_subscriber.unregister()
self.joint_state_subscriber = None
class TrajectoryControllerSpawner(object):
def __init__(self, trajectory, service_timeout):
self.trajectory = trajectory
self.service_timeout = service_timeout
self.hand_finder = HandFinder()
self.joints = self.hand_finder.get_hand_joints()
ros_pack = rospkg.RosPack()
sr_robot_launch_path = ros_pack.get_path('sr_robot_launch')
self.hand_mapping = self.hand_finder.get_hand_parameters().mapping
self.yaml_file_path = {}
for hand in self.hand_mapping:
self.yaml_file_path[self.hand_mapping[hand]] = (
sr_robot_launch_path + "/config/" + self.hand_mapping[hand] + "_trajectory_controller.yaml")
def generate_parameters(self):
for hand in self.yaml_file_path:
with open(self.yaml_file_path[hand], 'r') as yaml_file:
yaml_content = yaml.load(yaml_file)
if hand + "_trajectory_controller" not in yaml_content.keys():
rospy.logerr("there are errors opening trajectory controller yaml file")
else:
hand_trajectory = yaml_content[hand + "_trajectory_controller"]
if rospy.has_param('~exclude_wrist') and rospy.get_param('~exclude_wrist'):
hand_trajectory['joints'] = [s for s in self.joints[hand] if "WR" not in s]
else:
hand_trajectory['joints'] = self.joints[hand]
for joint_name in hand_trajectory['constraints'].keys():
if (joint_name not in hand_trajectory['joints'] and
joint_name != 'goal_time' and joint_name != 'stopped_velocity_tolerance'):
del hand_trajectory['constraints'][joint_name]
param_prefix = hand + "_trajectory_controller/"
rospy.set_param(param_prefix + 'allow_partial_joints_goal',
hand_trajectory['allow_partial_joints_goal'])
rospy.set_param(param_prefix + 'joints', hand_trajectory['joints'])
rospy.set_param(param_prefix + 'stop_trajectory_duration', hand_trajectory['stop_trajectory_duration'])
rospy.set_param(param_prefix + 'type', hand_trajectory['type'])
constrain_prefix = param_prefix + 'constraints/'
for constraint in hand_trajectory['constraints']:
if constraint == 'goal_time' or constraint == 'stopped_velocity_tolerance':
rospy.set_param(constrain_prefix + constraint, hand_trajectory['constraints'][constraint])
else:
rospy.set_param(constrain_prefix + constraint + '/goal',
hand_trajectory['constraints'][constraint]['goal'])
rospy.set_param(constrain_prefix + constraint + '/trajectory',
hand_trajectory['constraints'][constraint]['trajectory'])
@staticmethod
def check_joint(joint, controllers_to_start, controller_names):
if joint[3:5].lower() == 'th' or joint[3:5].lower() == 'wr' or (joint[6] != '1' and joint[6] != '2'):
joint_controller = 'sh_' + joint.lower() + "_position_controller"
else:
joint = joint[:6] + '0'
joint_controller = 'sh_' + joint.lower() + "_position_controller"
if joint_controller not in controller_names and joint_controller not in controllers_to_start:
controllers_to_start.append(joint_controller)
def set_controller(self):
controllers_to_start = []
for hand_serial in self.hand_mapping:
hand_prefix = self.hand_mapping[hand_serial]
success = True
try:
rospy.wait_for_service('controller_manager/list_controllers', self.service_timeout)
list_controllers = rospy.ServiceProxy(
'controller_manager/list_controllers', ListControllers)
running_controllers = list_controllers()
except rospy.ServiceException:
success = False
rospy.logerr("Failed to load trajectory controller")
if success:
already_running = False
controller_names = []
for controller_state in running_controllers.controller:
controller_names.append(controller_state.name)
if controller_state.name == hand_prefix + '_trajectory_controller':
already_running = True
if self.trajectory and not already_running:
controllers_to_start.append(hand_prefix + '_trajectory_controller')
for joint in self.joints[hand_prefix]:
TrajectoryControllerSpawner.check_joint(joint, controllers_to_start, controller_names)
for load_control in controllers_to_start:
try:
rospy.wait_for_service('controller_manager/load_controller', self.service_timeout)
load_controllers = rospy.ServiceProxy('controller_manager/load_controller', LoadController)
loaded_controllers = load_controllers(load_control)
except rospy.ServiceException:
success = False
if not loaded_controllers.ok:
success = False
try:
rospy.wait_for_service('controller_manager/switch_controller', self.service_timeout)
switch_controllers = rospy.ServiceProxy('controller_manager/switch_controller', SwitchController)
switched_controllers = switch_controllers(controllers_to_start, None,
SwitchController._request_class.BEST_EFFORT)
except rospy.ServiceException:
success = False
if not switched_controllers.ok:
success = False
if not success:
rospy.logerr("Failed to launch trajectory controller!")
@staticmethod
def wait_for_topic(topic_name, timeout):
if not topic_name:
return True
# This has to be a list since Python has a peculiar mechanism to determine
# whether a variable is local to a function or not:
# if the variable is assigned in the body of the function, then it is
# assumed to be local. Modifying a mutable object (like a list)
# works around this debatable "design choice".
wait_for_topic_result = [None]
def wait_for_topic_cb(msg):
wait_for_topic_result[0] = msg
rospy.logdebug("Heard from wait-for topic: %s" % str(msg.data))
rospy.Subscriber(topic_name, Bool, wait_for_topic_cb)
started_waiting = rospy.Time.now().to_sec()
# We might not have received any time messages yet
warned_about_not_hearing_anything = False
while not wait_for_topic_result[0]:
rospy.sleep(0.01)
if rospy.is_shutdown():
return False
if not warned_about_not_hearing_anything:
if rospy.Time.now().to_sec() - started_waiting > timeout:
warned_about_not_hearing_anything = True
rospy.logwarn("Controller Spawner hasn't heard anything from its \"wait for\" topic (%s)" %
topic_name)
while not wait_for_topic_result[0].data:
rospy.sleep(0.01)
if rospy.is_shutdown():
return False
return True