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