From e3db1f1409e7a8a36bb9129898a1a4c5ef72b106 Mon Sep 17 00:00:00 2001 From: Manuel Stahl Date: Wed, 12 Aug 2020 13:23:36 +0200 Subject: [PATCH 1/7] Fix joint_trajectory_client: Add missing point.velocities in add_point() --- intera_examples/scripts/joint_trajectory_client.py | 1 + 1 file changed, 1 insertion(+) diff --git a/intera_examples/scripts/joint_trajectory_client.py b/intera_examples/scripts/joint_trajectory_client.py index 6ca1093..4d0ff41 100755 --- a/intera_examples/scripts/joint_trajectory_client.py +++ b/intera_examples/scripts/joint_trajectory_client.py @@ -62,6 +62,7 @@ def __init__(self, limb, joint_names): def add_point(self, positions, time): point = JointTrajectoryPoint() point.positions = copy(positions) + point.velocities = [0.1] * len(positions) point.time_from_start = rospy.Duration(time) self._goal.trajectory.points.append(point) From bca05b2c4396dc7b77812e69184bb186f42f9aa3 Mon Sep 17 00:00:00 2001 From: Manuel Stahl Date: Wed, 12 Aug 2020 13:26:22 +0200 Subject: [PATCH 2/7] Fix limb: goal.names must be goal.name See http://docs.ros.org/kinetic/api/sensor_msgs/html/msg/JointState.html --- intera_interface/src/intera_interface/limb.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/intera_interface/src/intera_interface/limb.py b/intera_interface/src/intera_interface/limb.py index 1a10163..3ffebcf 100644 --- a/intera_interface/src/intera_interface/limb.py +++ b/intera_interface/src/intera_interface/limb.py @@ -625,7 +625,7 @@ def ik_request(self, pose, ikreq.use_nullspace_goal.append(True) # The nullspace goal can either be the full set or subset of joint angles goal = JointState() - goal.names = nullspace_goal.keys() + goal.name = nullspace_goal.keys() goal.position = nullspace_goal.values() ikreq.nullspace_goal.append(goal) # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0] From f407e469ac3f64edc5f2c77cb3d42ff9364f55a6 Mon Sep 17 00:00:00 2001 From: Manuel Stahl Date: Wed, 12 Aug 2020 13:26:30 +0200 Subject: [PATCH 3/7] Fix coding style --- .../src/intera_motion_interface/motion_waypoint.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/intera_interface/src/intera_motion_interface/motion_waypoint.py b/intera_interface/src/intera_motion_interface/motion_waypoint.py index 23aa437..7c0c435 100644 --- a/intera_interface/src/intera_motion_interface/motion_waypoint.py +++ b/intera_interface/src/intera_motion_interface/motion_waypoint.py @@ -164,9 +164,9 @@ def set_cartesian_pose(self, pose = None, This is used as the nullspace bias if set. """ if pose is None: - self._data.pose = PoseStamped() # empty pose + self._data.pose = PoseStamped() # empty pose else: - self._data.pose = pose + self._data.pose = pose if active_endpoint is None: active_endpoint = MotionWaypoint.get_default_active_endpoint() From d2a87cfb5dd2f93adf19d585486f321a0c504d6d Mon Sep 17 00:00:00 2001 From: Manuel Stahl Date: Tue, 13 Oct 2020 15:58:12 +0200 Subject: [PATCH 4/7] Fix MotionTrajectory pose check --- .../src/intera_motion_interface/motion_trajectory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/intera_interface/src/intera_motion_interface/motion_trajectory.py b/intera_interface/src/intera_motion_interface/motion_trajectory.py index 9e90a46..2a1cbe0 100644 --- a/intera_interface/src/intera_motion_interface/motion_trajectory.py +++ b/intera_interface/src/intera_motion_interface/motion_trajectory.py @@ -282,7 +282,7 @@ def _check_cartesian_options(self): orientation = [pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z] # If the endpoint pose has not been set - if not sum(position) or not sum(orientation): + if None in position or all(v == 0 for v in orientation): new_wpt = MotionWaypoint(options = wpt.options, limb = self._limb) new_wpt.set_joint_angles(joint_angles = wpt.joint_positions, active_endpoint = wpt.active_endpoint, From cd831ea7a69de5ebdf2a81d3f157f6efc6a9861c Mon Sep 17 00:00:00 2001 From: Manuel Stahl Date: Sat, 26 Dec 2020 13:55:45 +0000 Subject: [PATCH 5/7] Fix io_config_editor.py JSON dump --- intera_interface/scripts/io_config_editor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/intera_interface/scripts/io_config_editor.py b/intera_interface/scripts/io_config_editor.py index 83fc185..44d3a09 100755 --- a/intera_interface/scripts/io_config_editor.py +++ b/intera_interface/scripts/io_config_editor.py @@ -51,7 +51,7 @@ def save_config(device, filename): # parse string so output can be nicely formatted json_config = json.loads(config) with open(filename, 'w') as f: - json.dump(json_config, f, ensure_ascii=False, sort_keys=True, + json.dump(json_config, f, ensure_ascii=True, sort_keys=True, indent=2, separators=(",", ": ")) else: rospy.logerr("Bad filename: '{}'".format(filename)) From f17f2a864f73effe0345e30e448b4020a058047f Mon Sep 17 00:00:00 2001 From: Manuel Stahl Date: Sat, 26 Dec 2020 13:56:02 +0000 Subject: [PATCH 6/7] limb.py: Don't subscribe to tip_states --- intera_interface/src/intera_interface/limb.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/intera_interface/src/intera_interface/limb.py b/intera_interface/src/intera_interface/limb.py index 3ffebcf..2e7a40b 100644 --- a/intera_interface/src/intera_interface/limb.py +++ b/intera_interface/src/intera_interface/limb.py @@ -138,12 +138,12 @@ def __init__(self, limb="right", synchronous_pub=False): queue_size=1, tcp_nodelay=True) - _tip_states_sub = rospy.Subscriber( - ns + 'tip_states', - EndpointStates, - self._on_tip_states, - queue_size=1, - tcp_nodelay=True) + #_tip_states_sub = rospy.Subscriber( + # ns + 'tip_states', + # EndpointStates, + # self._on_tip_states, + # queue_size=1, + # tcp_nodelay=True) _collision_state_sub = rospy.Subscriber( ns + 'collision_detection_state', @@ -176,8 +176,8 @@ def __init__(self, limb="right", synchronous_pub=False): timeout_msg=err_msg, timeout=5.0) err_msg = ("%s limb init failed to get current tip_states " "from %s") % (self.name.capitalize(), ns + 'tip_states') - intera_dataflow.wait_for(lambda: self._tip_states is not None, - timeout_msg=err_msg, timeout=5.0) + #intera_dataflow.wait_for(lambda: self._tip_states is not None, + # timeout_msg=err_msg, timeout=5.0) def _on_joint_states(self, msg): for idx, name in enumerate(msg.name): From 080c8a5d2f47207498ae470e57c1ea81ef710234 Mon Sep 17 00:00:00 2001 From: Manuel Stahl Date: Sat, 26 Dec 2020 14:48:46 +0100 Subject: [PATCH 7/7] [sim] Wait longer for IF and FK services --- intera_interface/src/intera_interface/limb.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/intera_interface/src/intera_interface/limb.py b/intera_interface/src/intera_interface/limb.py index 2e7a40b..2be0d30 100644 --- a/intera_interface/src/intera_interface/limb.py +++ b/intera_interface/src/intera_interface/limb.py @@ -163,8 +163,8 @@ def __init__(self, limb="right", synchronous_pub=False): ns_pkn = "ExternalTools/" + limb + "/PositionKinematicsNode/" self._iksvc = rospy.ServiceProxy(ns_pkn + 'IKService', SolvePositionIK) self._fksvc = rospy.ServiceProxy(ns_pkn + 'FKService', SolvePositionFK) - rospy.wait_for_service(ns_pkn + 'IKService', 5.0) - rospy.wait_for_service(ns_pkn + 'FKService', 5.0) + rospy.wait_for_service(ns_pkn + 'IKService', 60.0) + rospy.wait_for_service(ns_pkn + 'FKService', 60.0) err_msg = ("%s limb init failed to get current joint_states " "from %s") % (self.name.capitalize(), joint_state_topic)