Some bugfixes

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)
 

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

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]

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,

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