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