Ejemplo n.º 1
0
    def compute_cartesian_path(self,
                               waypoints,
                               eef_step,
                               jump_threshold,
                               avoid_collisions=True,
                               path_constraints=None):
        """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
        if path_constraints:
            if type(path_constraints) is Constraints:
                constraints_str = conversions.msg_to_string(path_constraints)
            else:
                raise MoveItCommanderException(
                    "Unable to set path constraints, unknown constraint type "
                    + type(path_constraints))
            (ser_path, fraction) = self._g.compute_cartesian_path(
                [conversions.pose_to_list(p) for p in waypoints], eef_step,
                jump_threshold, avoid_collisions, constraints_str)
        else:
            (ser_path, fraction) = self._g.compute_cartesian_path(
                [conversions.pose_to_list(p) for p in waypoints], eef_step,
                jump_threshold, avoid_collisions)

        path = RobotTrajectory()
        path.deserialize(ser_path)
        return (path, fraction)
Ejemplo n.º 2
0
 def place(self, object_name, pose):
     """Place the named object at a particular location in the environment"""
     result = False
     if type(pose) is PoseStamped:
         old = self.get_pose_reference_frame()
         self.set_pose_reference_frame(pose.header.frame_id)
         result = self._g.place(object_name, conversions.pose_to_list(pose.pose))
         self.set_pose_reference_frame(old)
     else:
         result = self._g.place(object_name, conversions.pose_to_list(pose))
     return result
Ejemplo n.º 3
0
 def place(self, object_name, pose):
     """Place the named object at a particular location in the environment"""
     result = False
     if type(pose) is PoseStamped:
         old = self.get_pose_reference_frame()
         self.set_pose_reference_frame(pose.header.frame_id)
         result = self._g.place(object_name,
                                conversions.pose_to_list(pose.pose))
         self.set_pose_reference_frame(old)
     else:
         result = self._g.place(object_name, conversions.pose_to_list(pose))
     return result
Ejemplo n.º 4
0
 def set_pose_target(self, pose, end_effector_link = ""):
     """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
     """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
     if len(end_effector_link) > 0 or self.has_end_effector_link():
         if type(pose) is PoseStamped:
             old = self.get_pose_reference_frame()
             self.set_pose_reference_frame(pose.header.frame_id)
             self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link)
             self.set_pose_reference_frame(old)
         elif type(pose) is Pose:
             self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link)
         else:
             self._g.set_pose_target(pose, end_effector_link)
     else:
         raise MoveItCommanderException("There is no end effector to set the pose for")
Ejemplo n.º 5
0
 def set_pose_targets(self, poses, end_effector_link = ""):
     """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
     if len(end_effector_link) > 0 or self.has_end_effector_link():
         if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link):
             raise MoveItCommanderException("Unable to set target poses")
     else:
         raise MoveItCommanderException("There is no end effector to set poses for")
Ejemplo n.º 6
0
 def set_pose_targets(self, poses, end_effector_link = ""):
     """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
     if len(end_effector_link) > 0 or self.has_end_effector_link():
         if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link):
             raise MoveItCommanderException("Unable to set target poses")
     else:
         raise MoveItCommanderException("There is no end effector to set poses for")
Ejemplo n.º 7
0
 def place(self, object_name, location=None):
     """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided"""
     result = False
     if location is None:
         result = self._g.place(object_name)
     elif type(location) is PoseStamped:
         old = self.get_pose_reference_frame()
         self.set_pose_reference_frame(location.header.frame_id)
         result = self._g.place(object_name, conversions.pose_to_list(location.pose))
         self.set_pose_reference_frame(old)
     elif type(location) is Pose:
         result = self._g.place(object_name, conversions.pose_to_list(location))
     elif type(location) is PlaceLocation:
         result = self._g.place(object_name, conversions.msg_to_string(location))
     else:
         raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object")
     return result
Ejemplo n.º 8
0
 def place(self, object_name, location=None):
     """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided"""
     result = False
     if location is None:
         result = self._g.place(object_name)
     elif type(location) is PoseStamped:
         old = self.get_pose_reference_frame()
         self.set_pose_reference_frame(location.header.frame_id)
         result = self._g.place(object_name, conversions.pose_to_list(location.pose))
         self.set_pose_reference_frame(old)
     elif type(location) is Pose:
         result = self._g.place(object_name, conversions.pose_to_list(location))
     elif type(location) is PlaceLocation:
         result = self._g.place(object_name, conversions.msg_to_string(location))
     else:
         raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object")
     return result
Ejemplo n.º 9
0
 def set_pose_target(self, pose, end_effector_link = ""):
     """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
     """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
     if len(end_effector_link) > 0 or self.has_end_effector_link():
         ok = False
         if type(pose) is PoseStamped:
             old = self.get_pose_reference_frame()
             self.set_pose_reference_frame(pose.header.frame_id)
             ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link)
             self.set_pose_reference_frame(old)
         elif type(pose) is Pose:
             ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link)
         else:
             ok = self._g.set_pose_target(pose, end_effector_link)
         if not ok:
             raise MoveItCommanderException("Unable to set target pose")
     else:
         raise MoveItCommanderException("There is no end effector to set the pose for")
Ejemplo n.º 10
0
 def compute_cartesian_path(self,
                            waypoints,
                            eef_step,
                            jump_threshold,
                            avoid_collisions=True):
     """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
     (ser_path, fraction) = self._g.compute_cartesian_path(
         [conversions.pose_to_list(p) for p in waypoints], eef_step,
         jump_threshold, avoid_collisions)
     path = RobotTrajectory()
     path.deserialize(ser_path)
     return (path, fraction)
Ejemplo n.º 11
0
 def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions = True):
     """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
     (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
     path = RobotTrajectory()
     path.deserialize(ser_path)
     return (path, fraction)
Ejemplo n.º 12
0
 def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions = True):
     (dpath, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
     return (conversions.dict_to_trajectory(dpath), fraction)