def goto_mp(self, movement_primitive: MovementPrimitive, frequency=50, duration=10., group_name="arm_gripper"): curr_controller = self._ctrl_manager.set_motion_controller( self._ctrl_manager.joint_trajectory_controller) traj_client = JointTrajectoryActionClient( joint_names=self.groups[group_name].refs) traj_client.clear() pos_traj = movement_primitive.get_full_trajectory(frequency=frequency, duration=duration) vel_traj = movement_primitive.get_full_trajectory_derivative( frequency=frequency, duration=duration) timing = [] positions = [] velocities = [] d_tot = 0 for goal, d in pos_traj: d_tot += float(d) timing.append(d_tot) positions.append([goal[k] for k in self.groups[group_name].refs]) for goal, d in vel_traj: velocities.append( [goal[k] / duration for k in self.groups[group_name].refs]) for t, position, velocity in zip(timing, positions, velocities): traj_client.add_point(positions=position, time=t, velocities=velocity) print("COMMAND SENT") traj_client.start() # send the trajectory action request # traj_client.wait(timeout = timeout) franka_dataflow.wait_for(test=lambda: traj_client.result(), timeout=60, timeout_msg="Something did not work", rate=100, raise_on_error=False) res = traj_client.result() if res is not None and res.error_code: rospy.loginfo("Trajectory Server Message: {}".format(res)) rospy.sleep(0.5) self._ctrl_manager.set_motion_controller(curr_controller) rospy.loginfo("{}: Trajectory controlling complete".format( self.__class__.__name__)) rospy.sleep(0.5)
def move_from_touch(self, positions, timeout=10.0, threshold=0.00085): """ (Blocking) Commands the limb to the provided positions. Waits until the reported joint state matches that specified. This function uses a low-pass filter to smooth the movement. @type positions: dict({str:float}) @param positions: joint_name:angle command @type timeout: float @param timeout: seconds to wait for move to finish [15] @type threshold: float @param threshold: position threshold in radians across each joint when move is considered successful [0.008726646] @param test: optional function returning True if motion must be aborted """ if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller: self.switchToController( self._ctrl_manager.joint_trajectory_controller) min_traj_dur = 0.5 traj_client = JointTrajectoryActionClient( joint_names=self.joint_names()) traj_client.clear() dur = [] for j in range(len(self._joint_names)): dur.append( max( abs(positions[self._joint_names[j]] - self._joint_angle[self._joint_names[j]]) / self._joint_limits.velocity[j], min_traj_dur)) traj_client.add_point( positions=[positions[n] for n in self._joint_names], time=max(dur) / self._speed_ratio) diffs = [ self.genf(j, a) for j, a in positions.items() if j in self._joint_angle ] fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format( self.name.capitalize()) traj_client.start() # send the trajectory action request franka_dataflow.wait_for(test=lambda: (all(diff() < threshold for diff in diffs)), timeout=timeout, timeout_msg="Unable to complete plan!", rate=100, raise_on_error=False) rospy.sleep(0.5) rospy.loginfo("ArmInterface: Trajectory controlling complete")
def move_to_joint_positions(self, positions, timeout=10.0, threshold=0.00085, test=None, use_moveit=True): """ (Blocking) Commands the limb to the provided positions. Waits until the reported joint state matches that specified. This function uses a low-pass filter using JointTrajectoryService to smooth the movement or optionally uses MoveIt! to plan and execute a trajectory. :type positions: dict({str:float}) :param positions: joint_name:angle command :type timeout: float :param timeout: seconds to wait for move to finish [15] :type threshold: float :param threshold: position threshold in radians across each joint when move is considered successful [0.00085] :param test: optional function returning True if motion must be aborted :type use_moveit: bool :param use_moveit: if set to True, and movegroup interface is available, move to the joint positions using moveit planner. """ curr_controller = self._ctrl_manager.set_motion_controller( self._ctrl_manager.joint_trajectory_controller) if use_moveit and self._movegroup_interface: self._movegroup_interface.go_to_joint_positions( [positions[n] for n in self._joint_names], tolerance=threshold) else: if use_moveit: rospy.logwarn( "{}: MoveGroupInterface was not found! Using JointTrajectoryActionClient instead." .format(self.__class__.__name__)) min_traj_dur = 0.5 traj_client = JointTrajectoryActionClient( joint_names=self.joint_names()) traj_client.clear() dur = [] for j in range(len(self._joint_names)): dur.append( max( abs(positions[self._joint_names[j]] - self._joint_angle[self._joint_names[j]]) / self._joint_limits.velocity[j], min_traj_dur)) traj_client.add_point( positions=[self._joint_angle[n] for n in self._joint_names], time=0.0001) traj_client.add_point( positions=[positions[n] for n in self._joint_names], time=max(dur) / self._speed_ratio) def genf(joint, angle): def joint_diff(): return abs(angle - self._joint_angle[joint]) return joint_diff diffs = [ genf(j, a) for j, a in positions.items() if j in self._joint_angle ] fail_msg = "{}: {} limb failed to reach commanded joint positions.".format( self.__class__.__name__, self.name.capitalize()) def test_collision(): if self.has_collided(): rospy.logerr(' '.join(["Collision detected.", fail_msg])) return True return False traj_client.start() # send the trajectory action request # traj_client.wait(timeout = timeout) franka_dataflow.wait_for( test=lambda: test_collision() or traj_client.result( ) is not None or (callable(test) and test() == True) or (all(diff() < threshold for diff in diffs)), timeout=timeout, timeout_msg=fail_msg, rate=100, raise_on_error=False) res = traj_client.result() if res is not None and res.error_code: rospy.loginfo("Trajectory Server Message: {}".format(res)) rospy.sleep(0.5) self._ctrl_manager.set_motion_controller(curr_controller) rospy.loginfo("{}: Trajectory controlling complete".format( self.__class__.__name__))
def go_to(self, trajectory: NamedTrajectoryBase, group_name="arm_gripper", frequency=50): """ (Blocking) Commands the limb to the provided positions. Waits until the reported joint state matches that specified. This function uses a low-pass filter using JointTrajectoryService to smooth the movement or optionally uses MoveIt! to plan and execute a trajectory. :type positions: dict({str:float}) :param positions: joint_name:angle command :type timeout: float :param timeout: seconds to wait for move to finish [15] :type threshold: float :param threshold: position threshold in radians across each joint when move is considered successful [0.00085] :param test: optional function returning True if motion must be aborted :type use_moveit: bool :param use_moveit: if set to True, and movegroup interface is available, move to the joint positions using moveit planner. """ curr_controller = self._ctrl_manager.set_motion_controller( self._ctrl_manager.joint_trajectory_controller) # traj_client = JointTrajectoryActionClient( # joint_names=self.joint_names()) traj_client = JointTrajectoryActionClient( joint_names=self.groups[group_name].refs) traj_client.clear() if len(trajectory.duration) > 1: self._get_cubic_spline(trajectory, group_name) for i, (position, velocity, d) \ in enumerate(zip(*self._get_cubic_spline(trajectory, group_name, frequency=frequency))): traj_client.add_point(positions=position.tolist(), time=float(d), velocities=velocity.tolist()) else: for goal, d in trajectory: position = [ float(goal[k]) for k in self.groups[group_name].refs ] velocity = [0.] * len(position) print("position", position) print("velocity", velocity) print("d", float(d)) traj_client.add_point(positions=position, time=float(d), velocities=velocity) traj_client.start() # send the trajectory action request # traj_client.wait(timeout = timeout) franka_dataflow.wait_for(test=lambda: traj_client.result(), timeout=260, timeout_msg="Something did not work", rate=100, raise_on_error=False) res = traj_client.result() if res is not None and res.error_code: rospy.loginfo("Trajectory Server Message: {}".format(res)) rospy.sleep(0.5) self._ctrl_manager.set_motion_controller(curr_controller) rospy.loginfo("{}: Trajectory controlling complete".format( self.__class__.__name__)) rospy.sleep(0.5)
def move_to_joint_positions(self, positions, timeout=10.0, threshold=0.00085, test=None): """ (Blocking) Commands the limb to the provided positions. Waits until the reported joint state matches that specified. This function uses a low-pass filter to smooth the movement. @type positions: dict({str:float}) @param positions: joint_name:angle command @type timeout: float @param timeout: seconds to wait for move to finish [15] @type threshold: float @param threshold: position threshold in radians across each joint when move is considered successful [0.008726646] @param test: optional function returning True if motion must be aborted """ if self._params._in_sim: rospy.warn( "ArmInterface: move_to_joint_positions not implemented for simulation. Use set_joint_positions instead." ) return switch_ctrl = True if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller else False if switch_ctrl: active_controllers = self._ctrl_manager.list_active_controllers( only_motion_controllers=True) for ctrlr in active_controllers: self._ctrl_manager.stop_controller(ctrlr.name) rospy.loginfo( "ArmInterface: Stopping %s for trajectory controlling" % ctrlr.name) rospy.sleep(0.5) if not self._ctrl_manager.is_loaded( self._ctrl_manager.joint_trajectory_controller): self._ctrl_manager.load_controller( self._ctrl_manager.joint_trajectory_controller) # if self._ctrl_manager.joint_trajectory_controller not in self._ctrl_manager.list_active_controller_names(): self._ctrl_manager.start_controller( self._ctrl_manager.joint_trajectory_controller) min_traj_dur = 0.5 traj_client = JointTrajectoryActionClient( joint_names=self.joint_names(), ns=self._ns) traj_client.clear() dur = [] for j in range(len(self._joint_names)): dur.append( max( abs(positions[self._joint_names[j]] - self._joint_angle[self._joint_names[j]]) / self._joint_limits.velocity[j], min_traj_dur)) traj_client.add_point( positions=[positions[n] for n in self._joint_names], time=max(dur) / self._speed_ratio) def genf(joint, angle): def joint_diff(): return abs(angle - self._joint_angle[joint]) return joint_diff diffs = [ genf(j, a) for j, a in positions.items() if j in self._joint_angle ] fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format( self.name.capitalize()) def test_collision(): if self.has_collided(): rospy.logerr(' '.join(["Collision detected.", fail_msg])) return True return False traj_client.start() # send the trajectory action request # traj_client.wait(timeout = timeout) franka_dataflow.wait_for( test=lambda: test_collision() or \ (callable(test) and test() == True) or \ (all(diff() < threshold for diff in diffs)), timeout=timeout, timeout_msg=fail_msg, rate=100, raise_on_error=False ) rospy.sleep(0.5) if switch_ctrl: self._ctrl_manager.stop_controller( self._ctrl_manager.joint_trajectory_controller) for ctrlr in active_controllers: self._ctrl_manager.start_controller(ctrlr.name) rospy.loginfo("ArmInterface: Restaring %s" % ctrlr.name) rospy.sleep(0.5) rospy.loginfo("ArmInterface: Trajectory controlling complete")
def move_to_touch(self, positions, timeout=10.0, threshold=0.00085): """ (Blocking) Commands the limb to the provided positions. Waits until the reported joint state matches that specified. This function uses a low-pass filter to smooth the movement. @type positions: dict({str:float}) @param positions: joint_name:angle command @type timeout: float @param timeout: seconds to wait for move to finish [15] @type threshold: float @param threshold: position threshold in radians across each joint when move is considered successful [0.008726646] @param test: optional function returning True if motion must be aborted """ if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller: self.switchToController( self._ctrl_manager.joint_trajectory_controller) min_traj_dur = 0.5 traj_client = JointTrajectoryActionClient( joint_names=self.joint_names()) traj_client.clear() speed_ratio = 0.05 # Move slower when approaching contact dur = [] for j in range(len(self._joint_names)): dur.append( max( abs(positions[self._joint_names[j]] - self._joint_angle[self._joint_names[j]]) / self._joint_limits.velocity[j], min_traj_dur)) traj_client.add_point( positions=[positions[n] for n in self._joint_names], time=max(dur) / speed_ratio, velocities=[0.002 for n in self._joint_names]) diffs = [ self.genf(j, a) for j, a in positions.items() if j in self._joint_angle ] fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format( self.name.capitalize()) traj_client.start() # send the trajectory action request franka_dataflow.wait_for( test=lambda: self.has_collided() or \ (all(diff() < threshold for diff in diffs)), timeout=timeout, timeout_msg="Move to touch complete.", rate=100, raise_on_error=False ) rospy.sleep(0.5) if not self.has_collided(): rospy.logerr('Move To Touch did not end in making contact') else: rospy.loginfo('Collision detected!') # The collision, though desirable, triggers a cartesian reflex error. We need to reset that error if self._robot_mode == 4: self.resetErrors() rospy.loginfo("ArmInterface: Trajectory controlling complete")
def execute_position_path(self, position_path, timeout=15.0, threshold=0.00085, test=None): """ (Blocking) Commands the limb to the provided positions. Waits until the reported joint state matches that specified. This function uses a low-pass filter to smooth the movement. @type positions: dict({str:float}) @param positions: joint_name:angle command @type timeout: float @param timeout: seconds to wait for move to finish [15] @type threshold: float @param threshold: position threshold in radians across each joint when move is considered successful [0.008726646] @param test: optional function returning True if motion must be aborted """ current_q = self.joint_angles() diff_from_start = sum( [abs(a - current_q[j]) for j, a in position_path[0].items()]) if diff_from_start > 0.1: raise IOError("Robot not at start of trajectory") if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller: self.switchToController( self._ctrl_manager.joint_trajectory_controller) min_traj_dur = 0.5 traj_client = JointTrajectoryActionClient( joint_names=self.joint_names()) traj_client.clear() time_so_far = 0 # Start at the second waypoint because robot is already at first waypoint for i in xrange(1, len(position_path)): q = position_path[i] dur = [] for j in range(len(self._joint_names)): dur.append( max( abs(q[self._joint_names[j]] - self._joint_angle[self._joint_names[j]]) / self._joint_limits.velocity[j], min_traj_dur)) time_so_far += max(dur) / self._speed_ratio traj_client.add_point( positions=[q[n] for n in self._joint_names], time=time_so_far, velocities=[0.005 for n in self._joint_names]) diffs = [ self.genf(j, a) for j, a in (position_path[-1]).items() if j in self._joint_angle ] # Measures diff to last waypoint fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format( self.name.capitalize()) def test_collision(): if self.has_collided(): rospy.logerr(' '.join(["Collision detected.", fail_msg])) return True return False traj_client.start() # send the trajectory action request franka_dataflow.wait_for( test=lambda: test_collision() or \ (callable(test) and test() == True) or \ (all(diff() < threshold for diff in diffs)), #timeout=timeout, timeout=max(time_so_far, timeout), #XXX timeout_msg=fail_msg, rate=100, raise_on_error=False ) rospy.sleep(0.5) rospy.loginfo("ArmInterface: Trajectory controlling complete")
def move_to_joint_positions(self, positions, timeout=10.0, threshold=0.00085, test=None): """ (Blocking) Commands the limb to the provided positions. Waits until the reported joint state matches that specified. This function uses a low-pass filter using JointTrajectoryService to smooth the movement or optionally uses MoveIt! to plan and execute a trajectory. :type positions: dict({str:float}) :param positions: joint_name:angle command :type timeout: float :param timeout: seconds to wait for move to finish [15] :type threshold: float :param threshold: position threshold in radians across each joint when move is considered successful [0.00085] :param test: optional function returning True if motion must be aborted """ if self._ctrl_manager.current_controller != self._ctrl_manager.joint_trajectory_controller: self.switchToController( self._ctrl_manager.joint_trajectory_controller) min_traj_dur = 0.5 traj_client = JointTrajectoryActionClient( joint_names=self.joint_names()) traj_client.clear() dur = [] for j in range(len(self._joint_names)): dur.append( max( abs(positions[self._joint_names[j]] - self._joint_angle[self._joint_names[j]]) / self._joint_limits.velocity[j], min_traj_dur)) traj_client.add_point( positions=[positions[n] for n in self._joint_names], time=max(dur) / self._speed_ratio) diffs = [ self.genf(j, a) for j, a in positions.items() if j in self._joint_angle ] traj_client.start() # send the trajectory action request fail_msg = "ArmInterface: {0} limb failed to reach commanded joint positions.".format( self.name.capitalize()) def test_collision(): if self.has_collided(): rospy.logerr(' '.join(["Collision detected.", fail_msg])) return True return False franka_dataflow.wait_for( test=lambda: test_collision() or \ (callable(test) and test() == True) or \ (all(diff() < threshold for diff in diffs)), timeout=timeout, timeout_msg=fail_msg, rate=100, raise_on_error=False ) res = traj_client.result() if res is not None and res.error_code: rospy.loginfo("Trajectory Server Message: {}".format(res)) rospy.sleep(0.5) rospy.loginfo("ArmInterface: Trajectory controlling complete")