class PlanningScene(metaclass=Singleton):
""" Singleton providing a simplified interface to the planning scene """
def __init__(self):
self._objs_on_tray = set()
self.__psi__ = PlanningSceneInterface(synchronous=True)
rospy.sleep(0.25)
def add_tray(self, pose, size):
self.__psi__.add_box('tray', pose, size)
def remove_all(self, keep_objs_on_tray=False):
"""
Remove all objects from planning scene, optionally sparing those on the tray and the tray itself
"""
if keep_objs_on_tray:
objs_to_remove = set(self.__psi__.get_known_object_names()
) - self._objs_on_tray - {'tray'}
for obj in objs_to_remove:
self.__psi__.remove_world_object(obj)
else:
self.__psi__.remove_world_object()
def remove_obj(self, obj_name=None):
"""
Remove an object from planning scene, or all if no name is provided
"""
self.__psi__.remove_world_object(obj_name)
def displace_obj(self, obj_name, new_pose):
co = self.get_obj(obj_name)
co.header = new_pose.header
co.pose = new_pose.pose
co.operation = CollisionObject.MOVE
self.__psi__._pub_co.publish(co)
def move_obj_to_tray(self, obj_name, pose_on_tray):
self.displace_obj(obj_name, pose_on_tray)
self._objs_on_tray.add(obj_name)
return
def get_obj(self, obj_name):
objs = self.__psi__.get_objects([obj_name])
if not objs:
rospy.logerr("Object '%s' not found on planning scene")
return None
assert len(objs) == 1, "%d objects with the same name???" % len(objs)
return objs[obj_name]
from moveit_commander.planning_scene_interface import PlanningSceneInterface
#from moveit_commander import PlanningSceneInterface
from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped, PointStamped
from moveit_msgs.msg import OrientationConstraint, JointConstraint, VisibilityConstraint, Constraints
from sensor_msgs.msg import Image, CameraInfo
import cv2
from cv_bridge import CvBridge, CvBridgeError
import numpy as np
import image_geometry
import os
from MeshPly import MeshPly
from library import *
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('vs_move_arm', anonymous=True)
scene = PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm")
robot = moveit_commander.RobotCommander()
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
bridge = CvBridge()
rospy.sleep(2)
group.set_max_acceleration_scaling_factor(0.05)
group.set_max_velocity_scaling_factor(0.15)
# dummy initial pose
rotation = (np.pi / 2, -np.pi / 4, np.pi / 2)
orient = Quaternion(*tf.transformations.quaternion_from_euler(*rotation))
pose = Pose(Point(0.7, 0, 0.5), orient)
#!/usr/bin/env python
import sys, rospy, moveit_commander, tf
from moveit_commander.planning_scene_interface import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, Quaternion
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_scene', anonymous=True)
scene = PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
world_angle_offset = 0
rospy.sleep(2)
# a stand on which the target rests upon
box_pose = PoseStamped()
box_pose.header.frame_id = robot.get_planning_frame()
box_pose.header.stamp = rospy.Time.now()
box_pose.pose.position.x = 1
box_pose.pose.position.y = -0.085
box_pose.pose.position.z = 0.201
box_pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, world_angle_offset))
scene.add_box('box', box_pose, size=(0.18, 0.45, 0.402))
moveit_commander.roscpp_shutdown()
#!/usr/bin/env python
import sys, rospy, moveit_commander, tf
from moveit_commander.planning_scene_interface import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, Quaternion
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_scene', anonymous=True)
scene = PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
rospy.sleep(2)
world_angle_offset = 0
plane_pose = PoseStamped()
plane_pose.header.frame_id = robot.get_planning_frame()
plane_pose.header.stamp = rospy.Time.now()
plane_pose.pose.position.z = -0.0151
plane_pose.pose.position.y = 0.45
plane_pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, world_angle_offset))
scene.add_box('plane', plane_pose, size=(4, 2, 0.03))
# this is just a piece of marble that we don't want the robot to smash into
marble_pose = PoseStamped()
marble_pose.header.frame_id = robot.get_planning_frame()
marble_pose.header.stamp = rospy.Time.now()
marble_pose.pose.position.x = 0
marble_pose.pose.position.y = 0.2875
marble_pose.pose.position.z = 0.065
marble_pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, world_angle_offset))
def __init__(self):
self._objs_on_tray = set()
self.__psi__ = PlanningSceneInterface(synchronous=True)
rospy.sleep(0.25)
# Create node
rospy.init_node('test_pnp')
# Define camera poses
org_surface_pose = [0.65, 0.4, 0.15, np.pi, 0.0, 0.0]
new_surface_pose = [0.25, 0.85, 0.15, np.pi, 0.0, 0.0]
# Initialize robot and move left arm to initial pose.
eb.enable_robot()
limb = eb.Arm("left")
plan = pd.read_csv("my_plan.csv")
plan.poseFrom = plan.poseFrom.apply(ast.literal_eval)
plan.poseTo = plan.poseTo.apply(ast.literal_eval)
scene = PlanningSceneInterface()
box_pose = PoseStamped()
box_pose.header.frame_id = "world"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 10
box_pose.pose.position.z = 0
box_pose.pose.orientation.x = 0
box_pose.pose.orientation.y = 0
box_pose.pose.orientation.z = 0
box_pose.pose.orientation.w = 1
scene.add_box('obstacle', box_pose, size=(1, 1, 0.75))
box_pose2 = box_pose
box_pose2.pose.position.x = 10
box_pose2.pose.position.y = 0
scene.add_box('obstacle2', box_pose2, size=(1, 1, 0.75))