diff --git a/intera_examples/scripts/joint_trajectory_client.py b/intera_examples/scripts/joint_trajectory_client.py index f104eed..a4f060e 100755 --- a/intera_examples/scripts/joint_trajectory_client.py +++ b/intera_examples/scripts/joint_trajectory_client.py @@ -61,6 +61,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) diff --git a/intera_interface/scripts/io_config_editor.py b/intera_interface/scripts/io_config_editor.py index 771c6bf..e279978 100755 --- a/intera_interface/scripts/io_config_editor.py +++ b/intera_interface/scripts/io_config_editor.py @@ -50,7 +50,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)) diff --git a/intera_interface/src/intera_interface/limb.py b/intera_interface/src/intera_interface/limb.py index ed6c9db..4561b86 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', @@ -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) @@ -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): @@ -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 = list(nullspace_goal.keys()) + goal.name = list(nullspace_goal.keys()) goal.position = list(nullspace_goal.values()) ikreq.nullspace_goal.append(goal) # The gain used to bias toward the nullspace goal. Must be [0.0, 1.0] diff --git a/intera_interface/src/intera_motion_interface/motion_trajectory.py b/intera_interface/src/intera_motion_interface/motion_trajectory.py index d51c93b..bb28852 100644 --- a/intera_interface/src/intera_motion_interface/motion_trajectory.py +++ b/intera_interface/src/intera_motion_interface/motion_trajectory.py @@ -281,7 +281,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, diff --git a/intera_interface/src/intera_motion_interface/motion_waypoint.py b/intera_interface/src/intera_motion_interface/motion_waypoint.py index 4cf8d92..80d6cc5 100644 --- a/intera_interface/src/intera_motion_interface/motion_waypoint.py +++ b/intera_interface/src/intera_motion_interface/motion_waypoint.py @@ -163,9 +163,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()