def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Pick object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
def main(argv):
rospy.init_node("Table_mesh_inserter")
scene = PlanningSceneInterface()
robot = RobotCommander()
while (scene._pub_co.get_num_connections() < 1):
pass
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.3250
p.pose.position.y = 0.4250
p.pose.position.z = -0.540
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
scene.add_box("table", p, (1.2, 1.2, 1.05))
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.0075
scene.add_box("plate", p, (0.25, 0.25, 0.015))
while not rospy.is_shutdown():
pass
def __init__(self):
self._scene = PlanningSceneInterface()
self.robot = RobotCommander()
# pause to wait for rviz to load
rospy.sleep(4)
self.first_stamp = None
self.cloud_pub = rospy.Publisher('/camera/depth_registered/points',
PointCloud2,
queue_size=20,
latch=True)
self.cloud_sub = rospy.Subscriber(
'/camera/depth_registered/points_old', PointCloud2, self.msg_cb)
# clear the scene
self._scene.remove_world_object()
# add floor object
floor_pose = [0, 0, -0.12, 0, 0, 0, 1]
floor_dimensions = [4, 4, 0.02]
self.add_box_object("floor", floor_dimensions, floor_pose)
# add collision objects
self._kinect_pose_1 = [0.0, 0.815, 0.665, 0, -0.707, 0.707, 0]
# stream the kinect tf
kinect_cloud_topic = "kinect2_rgb_optical_frame"
thread.start_new_thread(self.spin, (kinect_cloud_topic, ))
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.ur5 = MoveGroupCommander("manipulator")
self.gripper = MoveGroupCommander("end_effector")
print("======================================================")
print(self.robot.get_group_names())
print(self.robot.get_link_names())
print(self.robot.get_joint_names())
print(self.robot.get_planning_frame())
print(self.ur5.get_end_effector_link())
print("======================================================")
self.ur5.set_max_velocity_scaling_factor(0.2)
self.ur5.set_max_acceleration_scaling_factor(0.2)
self.ur5.set_end_effector_link("fts_toolside")
self.ur5.set_planning_time(10.0)
#self.ur5.set_planner_id("RRTkConfigDefault")
self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller')
self.gripper_ac.wait_for_server()
self.gripper_ac.initiate()
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb)
def get_group_for_joint(joint_names, robot=None, smallest=False, groups=None):
"""Given list of joints find the most appropriate group.
INPUT
link_ids [List of Strings]
robot [RobotCommander]
smallest [Bool]
OUTPUT
group [String]
"""
if not robot:
robot = RobotCommander()
if not groups:
groups = robot.get_group_names()
group = None
n = np.Inf if smallest else 0
for g in groups:
names = robot.get_joint_names(g)
if all_in(joint_names, names):
if smallest and len(names) < n:
n = len(names)
group = g
elif len(names) > n:
n = len(names)
group = g
return group
class PythonTimeParameterizationTest(unittest.TestCase):
PLANNING_GROUP = "arm"
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def plan(self):
start_pose = self.group.get_current_pose().pose
goal_pose = self.group.get_current_pose().pose
goal_pose.position.z -= 0.1
(plan,
fraction) = self.group.compute_cartesian_path([start_pose, goal_pose],
0.005, 0.0)
self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
return plan
def time_parameterization(self, plan):
ref_state = self.commander.get_current_state()
retimed_plan = self.group.retime_trajectory(
ref_state, plan, velocity_scaling_factor=0.1)
return retimed_plan
def test_plan_and_time_parameterization(self):
plan = self.plan()
retimed_plan = self.time_parameterization(plan)
def __init__(self):
allowed_planning_time = 0.0
ARM_AND_LINEAR_GROUP_NAME = "arm_and_linear"
ARM_GROUP_NAME = "arm_torso"
GRIPPER_GROUP = "gripper"
REFERENCE_FRAME = "base_link"
PLANNER = "RRTConnectkConfigDefault"
#PLANNER = "RRTstarkConfigDefault"
MOVE_GROUP_SERVICE = "/move_group/plan_execution/set_parameters"
result = False
upright_constraints = None
robot = None
arm = None
arm_and_linear = None
database = None
scene = None
grasp_generator = None
place_generator = None
marker_publisher = None
self.robot = RobotCommander()
self.arm = self.robot.get_group("arm_torso")
self.gripper = self.robot.get_group(GRIPPER_GROUP)
self.scene = PlanningSceneInterface()
self.pickup_object = RecognizedObject("part")
self.table = EntityInfo()
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def commander():
global past_reads
rospy.init_node('rubik_camera', anonymous=True)
rospy.Subscriber("/usb_cam/image_raw", Image, callback)
pub = rospy.Publisher("cube_configuration", String, queue_size=1)
# Gets a handle on the robot that MoveIt is controlling
robot = RobotCommander()
rospy.sleep(1)
# Picks a group called "manipulator" to work with #
arm = robot.get_group("manipulator")
arm.set_max_velocity_scaling_factor(1.0)
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.005)
go_home(arm)
pick_rubik(arm, robot, "right")
go_home(arm)
for i in range(5):
read(arm, i)
go_home(arm)
place_rubik(arm, robot, "right")
go_home(arm)
pick_rubik(arm, robot, "left")
go_home(arm)
for i in range(5):
read(arm, i)
go_home(arm)
place_rubik(arm, robot, "left")
go_home(arm)
print(str(past_reads))
pub.publish(reads_to_cube())
rospy.sleep(1)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback)
freq = 30
rate = rospy.Rate(freq)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
self.ii = 0
# self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.br = tf.TransformBroadcaster()
self.target_ar_id = 9
self.calculed_tomato = Pose()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def __init__(self):
self._scene = PlanningSceneInterface()
# clear the scene
self._scene.remove_world_object()
self.robot = RobotCommander()
# pause to wait for rviz to load
rospy.sleep(4)
floor_pose = [0, 0, -0.12, 0, 0, 0, 1]
floor_dimensions = [4, 4, 0.02]
box1_pose = [0.70, 0.70, 0.25, 0, 0, 0, 1]
box1_dimensions = [0.5, 0.5, 0.5]
box2_pose = [0.5, -0.5, 0.60, 0, 0, 0, 1]
box2_dimensions = [0.25, 0.25, 0.25]
box3_pose = [0.5, -0.5, 0.24, 0, 0, 0, 1]
box3_dimensions = [0.48, 0.48, 0.48]
box4_pose = [-0.8, 0.7, 0.5, 0, 0, 0, 1]
box4_dimensions = [0.4, 0.4, 0.4]
self.add_box_object("floor", floor_dimensions, floor_pose)
self.add_box_object("box1", box1_dimensions, box1_pose)
self.add_box_object("box2", box2_dimensions, box2_pose)
self.add_box_object("box3", box3_dimensions, box3_pose)
self.add_box_object("box4", box4_dimensions, box4_pose)
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
rospy.sleep(1.0)
# self.setup()
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
rospy.loginfo("added table")
rospy.sleep(2.0)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.loginfo("added grasping box")
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.06
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
def init_arm(self, num_planning_attempts=20):
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_tolerance(0.2)
self.init_services()
self.init_action_servers()
def get_current_state():
# Prepare a new state object for validity check
rs = RobotState()
robot = RobotCommander()
robot_state = robot.get_current_state()
rs.joint_state.name = robot_state.joint_state.name
rs.joint_state.position = list(robot_state.joint_state.position) # filler for rest of the joint angles not found in waypoint
return rs
def __init__(self):
roscpp_initialize([])
rospy.sleep(8)
# TO-DO: wait for moveit.
self.listener = tf.TransformListener()
self.robot = RobotCommander()
self.init_planner()
self.gripper_dimensions = [0.02, 0.02, 0.02]
def setUp(self):
self.robot = RobotCommander()
self.bounds = list()
for name in self.robot.get_joint_names():
joint = self.robot.get_joint(name)
bounds = joint.bounds()
if bounds:
self.bounds.append((name, bounds))
class PythonTimeParameterizationTest(unittest.TestCase):
PLANNING_GROUP = "manipulator"
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def plan(self):
start_pose = self.group.get_current_pose().pose
goal_pose = self.group.get_current_pose().pose
goal_pose.position.z -= 0.1
(plan, fraction) = self.group.compute_cartesian_path(
[start_pose, goal_pose], 0.005, 0.0
)
self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
return plan
def time_parameterization(self, plan, algorithm):
ref_state = self.commander.get_current_state()
retimed_plan = self.group.retime_trajectory(
ref_state,
plan,
velocity_scaling_factor=0.1,
acceleration_scaling_factor=0.1,
algorithm=algorithm,
)
return retimed_plan
def test_plan_and_time_parameterization(self):
plan = self.plan()
retimed_plan = self.time_parameterization(
plan, "iterative_time_parameterization"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(
plan, "iterative_spline_parameterization"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(
plan, "time_optimal_trajectory_generation"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(plan, "")
self.assertTrue(
len(retimed_plan.joint_trajectory.points) == 0, "Invalid retime algorithm"
)
def __init__(self):
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.p = PoseStamped()
self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
def init_scene(self):
world_objects = [
"table", "tui", "monitor", "overHead", "wall", "farWall",
"frontWall", "backWall", "blockProtector", "rearCamera"
]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8836)
self.overHeadPose = PoseStamped()
self.overHeadPose.header.frame_id = self.robot.get_planning_frame()
self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97)
self.overHeadDimension = (0.9906, 0.8382, 0.05)
self.blockProtectorPose = PoseStamped()
self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame(
)
self.blockProtectorPose.pose.position = Point(
0.0056, -0.343, -0.51 + self.cruising_height)
self.blockProtectorDimension = (0.9906, 0.8382, 0.8636)
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
self.farWallPose = PoseStamped()
self.farWallPose.header.frame_id = self.robot.get_planning_frame()
self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048)
self.farWallDimension = (0.6096, 2, 3.35)
self.frontWallPose = PoseStamped()
self.frontWallPose.header.frame_id = self.robot.get_planning_frame()
self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51)
self.frontWallDimension = (1, 0.15, 4)
self.backWallPose = PoseStamped()
self.backWallPose.header.frame_id = self.robot.get_planning_frame()
self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51)
self.backWallDimension = (1, 0.005, 4)
self.rearCameraPose = PoseStamped()
self.rearCameraPose.header.frame_id = self.robot.get_planning_frame()
self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51)
self.rearCameraDimension = (0.5, 0.5, 2)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
self.scene.add_box("farWall", self.farWallPose, self.farWallDimension)
self.scene.add_box("overHead", self.overHeadPose,
self.overHeadDimension)
self.scene.add_box("backWall", self.backWallPose,
self.backWallDimension)
self.scene.add_box("frontWall", self.frontWallPose,
self.frontWallDimension)
self.scene.add_box("rearCamera", self.rearCameraPose,
self.rearCameraDimension)
def __init__(self,
scene=None,
manip_name="manipulator",
eef_name="endeffector"):
self.robot = RobotCommander()
self.man = MoveGroupCommander(manip_name)
self.eef = MoveGroupCommander(eef_name)
self.scene = scene
self.target_poses = []
self.picked = []
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def setUpClass(self):
rospy.init_node('test_moveit_vs060')
self.robot = RobotCommander()
self._mvgroup = MoveGroupCommander(self._MOVEGROUP_MAIN)
# Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
self._mvgroup.set_planner_id(self._KINEMATICSOLVER_SAFE)
self._movegroups = sorted(['manipulator', 'manipulator_flange'])
self._joints_movegroup_main = sorted(
['j1', 'j2', 'j3', 'j4', 'j5', 'flange'])
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param(
'/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber(
'/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
self.distance = 0
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.calculated_tomato_coor = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def print_joint_bounds(robot=None):
if not robot:
robot = RobotCommander()
print 'NAME'.center(16), 'MIN'.center(10), 'MAX'.center(10)
for name in robot.get_joint_names():
joint = robot.get_joint(name)
bounds = joint.bounds()
if bounds:
print '%15s % 10.3f % 10.3f' % (name, bounds[0], bounds[1])
def init_arm(self, num_planning_attempts=700):
rospy.set_param(
"/move_group/trajectory_execution/allowed_start_tolerance",
self.START_TOLERANCE)
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE)
self.arm.set_goal_orientation_tolerance(0.02)
self.init_services()
self.init_action_servers()
def __init__(self):
rospy.loginfo("Waiting for service " + IK_SERVICE_NAME)
rospy.wait_for_service(IK_SERVICE_NAME)
self.ik_serv = rospy.ServiceProxy(IK_SERVICE_NAME, GetPositionIK)
self.joint_list = [
'torso_1_joint', 'torso_2_joint', 'head_1_joint', 'head_2_joint',
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint', 'arm_right_1_joint', 'arm_right_2_joint',
'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint',
'arm_right_6_joint', 'arm_right_7_joint'
]
self.left_arm = [
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint'
]
self.right_arm = [
'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint'
]
self.right_arm_torso = [
'torso_1_joint', 'torso_2_joint', 'arm_right_1_joint',
'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint',
'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint'
]
self.left_arm_torso = [
'torso_1_joint', 'torso_2_joint', 'arm_left_1_joint',
'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint',
'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint'
]
# # Store answer so we ask only one time
# self.joint_state = JointState()
# self.joint_state.name = gksi_answer.kinematic_solver_info.joint_names
# self.joint_state.position = [0.0] * len(gksi_answer.kinematic_solver_info.joint_names)
# self.ik_link_name = gksi_answer.kinematic_solver_info.link_names[0]
self.r_commander = RobotCommander()
self.initial_robot_state = self.r_commander.get_current_state()
self.pub_ok_markers = rospy.Publisher('ik_ok_marker_list',
MarkerArray,
latch=True)
self.ok_markers = MarkerArray()
self.pub_fail_markers = rospy.Publisher('ik_fail_marker_list',
MarkerArray,
latch=True)
self.fail_markers = MarkerArray()
self.markers_id = 5
def __init__(self, group_name='arms'):
""" Initialize the robot commander for particular group."""
MoveGroupCommander.__init__(self, group_name)
self.set_planner_id("RRTConnectkConfigDefault")
# Sleep in order to load an ik module
rospy.sleep(1)
self.robot = RobotCommander()
self._ik_use_tip_link = True
self._start_state = None
self.overwrite_time_parameterization = True
# Decrease the joint tolerance
self.set_goal_joint_tolerance(0.00001)
def __init__(self):
self._scene = PlanningSceneInterface()
# clear the scene
self._scene.remove_world_object()
self.robot = RobotCommander()
# pause to wait for rviz to load
print "============ Waiting while RVIZ displays the scene with obstacles..."
# TODO: need to replace this sleep by explicitly waiting for the scene to be updated.
rospy.sleep(2)
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object2_name = rospy.get_param('~grasp_object_name',
'Grasp_Object-2')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:创建（调试）发布者：
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:创建规划场景和机器人命令：
self._scene = PlanningSceneInterface() #未知
self._robot = RobotCommander() #未知
rospy.sleep(0.5)
# Clean the scene:清理现场：
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._grasp_object2_name)
self._scene.remove_world_object(self._table_object_name)
# Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景：
self._pose_table = self._add_table(self._table_object_name) #增加桌子
self._pose_coke_can = self._add_grasp_block_(
self._grasp_object_name) #增加障碍物块
self._pose_coke_can = self._add_grasp_fanfkuai_(
self._grasp_object2_name) #增加障碍物块
rospy.sleep(0.5)
def __init__(self):
rospy.wait_for_service("gazebo/spawn_sdf_model")
self.spawn_model_srv = rospy.ServiceProxy("gazebo/spawn_sdf_model",
SpawnModel)
rospy.wait_for_service("gazebo/spawn_sdf_model")
self.delete_model_srv = rospy.ServiceProxy("gazebo/delete_model",
DeleteModel)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(1)
self.object_list = {}
def __init__(self,
planner=default_planner,
links={"ee": (LinkType.INTERMEDIATE, "manipulator")}):
# initialize interface node (if needed)
if rospy.get_node_uri() is None:
self.node_name = 'ur5_interface_node'
rospy.init_node(self.node_name)
ROS_INFO(
"The Arm interface was created outside a ROS node, a new node will be initialized!"
)
else:
ROS_INFO(
"The Arm interface was created within a ROS node, no need to create a new one!"
)
# store parameters
self._planner = planner
self._links = links
# create a RobotCommander
self._robot = RobotCommander()
# create semaphore
self._semaphore = BoundedSemaphore(value=1)
# default link (for utility functions only)
self._default_link = None
# create links objects
num_valid_links = 0
for link in self._links.items():
link_name, link_description = link
link_type, link_group_name = link_description
link_class = Arm.link_type_map[link_type]
link_obj = link_class(self, link_group_name, self._planner)
if not self._robot.has_group(link_group_name):
ROS_ERR("Move group `%s` not found!", link_group_name)
else:
self.__dict__[link_name] = link_obj
self._default_link = link_obj
num_valid_links += 1
if num_valid_links <= 0:
ROS_ERR("No valid links were found")
return None
# keep track of the status of the node
self._is_shutdown = False
self.verbose = False
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
class PythonMoveitCommanderTest(unittest.TestCase):
PLANNING_GROUP = "manipulator"
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def check_target_setting(self, expect, *args):
if len(args) == 0:
args = [expect]
self.group.set_joint_value_target(*args)
res = self.group.get_joint_value_target()
self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)),
"Setting failed for %s, values: %s" % (type(args[0]), res))
def test_target_setting(self):
n = self.group.get_variable_count()
self.check_target_setting([0.1] * n)
self.check_target_setting((0.2,) * n)
self.check_target_setting(np.zeros(n))
self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()})
self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
def plan(self, target):
self.group.set_joint_value_target(target)
return self.group.plan()
def test_validation(self):
current = np.asarray(self.group.get_current_joint_values())
success1, plan1, time1, err1 = self.plan(current + 0.2)
success2, plan2, time2, err2 = self.plan(current + 0.2)
self.assertTrue(success1)
self.assertTrue(success2)
# first plan should execute
self.assertTrue(self.group.execute(plan1))
# second plan should be invalid now (due to modified start point) and rejected
self.assertFalse(self.group.execute(plan2))
# newly planned trajectory should execute again
success3, plan3, time3, err3 = self.plan(current)
self.assertTrue(success3)
self.assertTrue(self.group.execute(plan3))
def test_planning_scene_interface(self):
planning_scene = PlanningSceneInterface()
def main():
rospy.init_node('moveit_py_place', anonymous=True)
#right_arm.set_planner_id("KPIECEkConfigDefault");
scene = PlanningSceneInterface()
robot = RobotCommander()
#group = MoveGroupCommander("head")
right_arm = MoveGroupCommander("right_arm")
#right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.logwarn("cleaning world")
GRIPPER_FRAME = 'gripper_bracket_f2'
#scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.67
p.pose.position.y = -0.
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part")
n_attempts += 1
print "Attempts pickup: ", n_attempts
rospy.sleep(0.2)
#robot.right_arm.pick("part")
#right_arm.go()
rospy.sleep(5)
def __init__(self):
rospy.loginfo("Waiting for service " + IK_SERVICE_NAME)
rospy.wait_for_service(IK_SERVICE_NAME)
self.ik_serv = rospy.ServiceProxy(IK_SERVICE_NAME, GetPositionIK)
self.joint_list = ['torso_1_joint', 'torso_2_joint',
'head_1_joint', 'head_2_joint',
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint',
'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.left_arm = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint']
self.right_arm = ['arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.right_arm_torso = ['torso_1_joint', 'torso_2_joint',
'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.left_arm_torso = ['torso_1_joint', 'torso_2_joint',
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint']
# # Store answer so we ask only one time
# self.joint_state = JointState()
# self.joint_state.name = gksi_answer.kinematic_solver_info.joint_names
# self.joint_state.position = [0.0] * len(gksi_answer.kinematic_solver_info.joint_names)
# self.ik_link_name = gksi_answer.kinematic_solver_info.link_names[0]
self.r_commander = RobotCommander()
self.initial_robot_state = self.r_commander.get_current_state()
if DEBUG_MODE:
self.pub_ok_markers = rospy.Publisher('ik_ok_marker_list', MarkerArray, latch=True)
self.ok_markers = MarkerArray()
self.pub_fail_markers = rospy.Publisher('ik_fail_marker_list', MarkerArray, latch=True)
self.fail_markers = MarkerArray()
self.markers_id = 5
def simple_function():
rc = RobotCommander()
mgc = MoveGroupCommander("manipulator")
# print(rc.get_group_names())
# print(rc.get_group('manipulator'))
# exit()
eef_step = 0.01
jump_threshold = 2
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rc.get_current_state()
rospy.logwarn(rc.get_current_state())
sss.wait_for_input()
#rate = rospy.Rate(0.1) # 10hz
rate = rospy.Rate(1) # 10hz
rospy.sleep(1)
theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
while not rospy.is_shutdown():
approach_pose = PoseStamped()
approach_pose.header.frame_id = "table"
approach_pose.header.stamp = rospy.Time(0)
approach_pose.pose.position.x = 0.146
approach_pose.pose.position.y = 0.622
approach_pose.pose.position.z = 0.01
quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
approach_pose.pose.orientation.x = quat[0]
approach_pose.pose.orientation.y = quat[1]
approach_pose.pose.orientation.z = quat[2]
approach_pose.pose.orientation.w = quat[3]
# mgc.set_start_state_to_current_state()
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed before adding collision objects")
# else:
# rospy.logwarn("Planning succeeded before adding collision objects")
#
# rospy.logwarn("waiting for input to add dummy box")
# sss.wait_for_input()
#
box_dummy_pose = PoseStamped()
box_dummy_pose.header.frame_id = "table"
box_dummy_pose.pose.position.x = 0.147
box_dummy_pose.pose.position.y = 0.636
box_dummy_pose.pose.position.z = 0
psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC
# rospy.logwarn("I have added the box")
# rospy.sleep(1)
# rospy.logwarn("waiting for input to try planning with dummy box")
# sss.wait_for_input()
#
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
# else:
# rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#
rospy.logwarn("waiting for input to add end effector box box")
sss.wait_for_input()
# end effector collision object
link_attached_to_ef = "ee_link"
mb_ef_collisonobj = "metal_bracket"
mb_ef_pose = PoseStamped()
mb_ef_pose.header.frame_id = link_attached_to_ef
mb_ef_pose.pose.position.x = 0.065/2.0
mb_ef_pose.pose.position.y = 0.0
mb_ef_pose.pose.position.z = 0.0
mb_ef_size = (0.065,0.06,0.06)
psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
raw_input("0 hi")
mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
rospy.logwarn("I have added the attached box to the end effector")
rospy.sleep(1)
raw_input("1 hi")
rospy.logwarn(rc.get_current_state())
# mgc.attach_object(object_name, link_name, touch_links)
mgc.set_start_state_to_current_state()
rospy.logwarn(rc.get_current_state())
raw_input("2 hi")
rospy.logwarn("waiting for input to try planning with end effector box")
sss.wait_for_input()
(traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
if(frac_approach != 1):
rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
else:
rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
rospy.logwarn("waiting for input to try planning next loop")
sss.wait_for_input()
rate.sleep()
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped, Pose
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest, GetPositionIKResponse
from trajectory_msgs.msg import JointTrajectoryPoint
import tf
import copy
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
group = MoveGroupCommander("arm")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Robot Links for arm:"
print robot.get_link_names("arm")
print "============ Robot Links for gripper:"
print robot.get_link_names("gripper")
print group.get_end_effector_link()
print group.get_pose_reference_frame()
print "============ Printing robot state"
#print robot.get_current_state()
print "============"
tl = tf.TransformListener()
rospy.sleep(1)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object("right_gripper_link", "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("start1")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
#grasps = self.make_grasps(start_pose)
#result = False
#n_attempts = 0
# repeat until will succeed
#while result == False:
#result = robot.right_arm.pick("part", grasps)
#n_attempts += 1
#print "Attempts: ", n_attempts
#rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
class CokeCanPickAndPlace:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.01, 0.01, 0.009))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -1
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
from std_msgs.msg import String
from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped
#http://moveit.ros.org/doxygen/annotated.html
#https://github.com/ros-planning/moveit_commander
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)
#Print Current State
state = robot.get_current_variable_values()
print state
print "---------------------------------------"
print "CURRENT STATE RIGHT ARM"
print "---------------------------------------"
print state['r_joint1'], " ", state['r_joint2'], " " ,state['r_joint3'], " " ,state['r_joint4'], " " ,state['r_joint5'], " " ,state['r_joint6'], " " ,state['r_joint7']
class MoveGroupCommandInterpreter(object):
"""
Interpreter for simple commands
"""
DEFAULT_FILENAME = "move_group.cfg"
GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
"left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
"forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def get_active_group(self):
if len(self._group_name) > 0:
return self._gdict[self._group_name]
else:
return None
def get_loaded_groups(self):
return self._gdict.keys()
def execute(self, cmd):
cmd = self.resolve_command_alias(cmd)
result = self.execute_generic_command(cmd)
if result != None:
return result
else:
if len(self._group_name) > 0:
return self.execute_group_command(self._gdict[self._group_name], cmd)
else:
return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")
def execute_generic_command(self, cmd):
if os.path.isfile("cmd/" + cmd):
cmd = "load cmd/" + cmd
cmdlow = cmd.lower()
if cmdlow.startswith("use"):
if cmdlow == "use":
return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
clist = cmd.split()
clist[0] = clist[0].lower()
if len(clist) == 2:
if clist[0] == "use":
if clist[1] == "previous":
clist[1] = self._prev_group_name
if len(clist[1]) == 0:
return (MoveGroupInfoLevel.DEBUG, "OK")
if self._gdict.has_key(clist[1]):
self._prev_group_name = self._group_name
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
else:
try:
mg = MoveGroupCommander(clist[1])
self._gdict[clist[1]] = mg
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
elif cmdlow.startswith("trace"):
if cmdlow == "trace":
return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
clist = cmdlow.split()
if clist[0] == "trace" and clist[1] == "on":
self._trace = True
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[0] == "trace" and clist[1] == "off":
self._trace = False
return (MoveGroupInfoLevel.DEBUG, "OK")
elif cmdlow.startswith("load"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
if len(clist) == 2:
filename = clist[1]
if not os.path.isfile(filename):
filename = "cmd/" + filename
line_num = 0
line_content = ""
try:
f = open(filename, 'r')
for line in f:
line_num += 1
line = line.rstrip()
line_content = line
if self._trace:
print ("%s:%d: %s" % (filename, line_num, line_content))
comment = line.find("#")
if comment != -1:
line = line[0:comment].rstrip()
if line != "":
self.execute(line.rstrip())
line_content = ""
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e)))
else:
return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e))
except:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content))
else:
return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
elif cmdlow.startswith("save"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN, "Unable to parse save command")
if len(clist) == 2:
filename = clist[1]
try:
f = open(filename, 'w')
for gr in self._gdict.keys():
f.write("use " + gr + "\n")
known = self._gdict[gr].get_remembered_joint_values()
for v in known.keys():
f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n")
if self._db_host != None:
f.write("database " + self._db_host + " " + str(self._db_port) + "\n")
return (MoveGroupInfoLevel.DEBUG, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
else:
return None
def execute_group_command(self, g, cmdorig):
cmd = cmdorig.lower()
if cmd == "stop":
g.stop()
return (MoveGroupInfoLevel.DEBUG, "OK")
if cmd == "id":
return (MoveGroupInfoLevel.INFO, g.get_name())
if cmd == "help":
return (MoveGroupInfoLevel.INFO, self.get_help())
if cmd == "vars":
known = g.get_remembered_joint_values()
return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")
if cmd == "joints":
joints = g.get_joints()
return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")
if cmd == "show":
return self.command_show(g)
if cmd == "current":
return self.command_current(g)
if cmd == "ground":
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
# offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
pose.pose.position.z = -0.0501
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = self._robot.get_root_link()
self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
return (MoveGroupInfoLevel.INFO, "Added ground")
if cmd == "eef":
if len(g.get_end_effector_link()) > 0:
return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
else:
return (MoveGroupInfoLevel.INFO, "There is no end effector defined")
if cmd == "database":
if self._db_host == None:
return (MoveGroupInfoLevel.INFO, "Not connected to a database")
else:
return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
if cmd == "plan":
if self._last_plan != None:
return (MoveGroupInfoLevel.INFO, str(self._last_plan))
else:
return (MoveGroupInfoLevel.INFO, "No previous plan")
if cmd == "constrain":
g.clear_path_constraints()
return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
if cmd == "tol" or cmd == "tolerance":
return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())
if cmd == "time":
return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
if cmd == "execute":
if self._last_plan != None:
if g.execute(self._last_plan):
return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
else:
return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution")
else:
return (MoveGroupInfoLevel.WARN, "No motion plan computed")
# see if we have assignment between variables
assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
if assign_match:
known = g.get_remembered_joint_values()
if known.has_key(assign_match.group(2)):
g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)])
return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2))
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
# see if we have assignment of matlab-like vector syntax
set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
if set_match:
try:
g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()])
return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]")
# see if we have assignment of matlab-like element update
component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
if component_match:
known = g.get_remembered_joint_values()
if known.has_key(component_match.group(1)):
try:
val = known[component_match.group(1)]
val[int(component_match.group(2))] = float(component_match.group(3))
g.remember_joint_values(component_match.group(1), val)
return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
clist = cmdorig.split()
clist[0] = clist[0].lower()
# if this is an unknown one-word command, it is probably a variable
if len(clist) == 1:
known = g.get_remembered_joint_values()
if known.has_key(cmd):
return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
# command with one argument
if len(clist) == 2:
if clist[0] == "go":
self._last_plan = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]")
else:
try:
g.set_named_target(clist[1])
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
if clist[0] == "plan":
self._last_plan = None
vals = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
self._last_plan = g.plan()
else:
try:
g.set_named_target(clist[1])
self._last_plan = g.plan()
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
if self._last_plan != None:
if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0:
self._last_plan = None
dest_str = clist[1]
if vals != None:
dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
if self._last_plan != None:
return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
else:
return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str)
elif clist[0] == "pick":
self._last_plan = None
if g.pick(clist[1]):
return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1])
elif clist[0] == "place":
self._last_plan = None
if g.place(clist[1]):
return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1])
elif clist[0] == "planner":
g.set_planner_id(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
elif clist[0] == "record" or clist[0] == "rec":
g.remember_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1])
elif clist[0] == "rand" or clist[0] == "random":
g.remember_joint_values(clist[1], g.get_random_joint_values())
return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1])
elif clist[0] == "del" or clist[0] == "delete":
g.forget_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1])
elif clist[0] == "show":
known = g.get_remembered_joint_values()
if known.has_key(clist[1]):
return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]")
else:
return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known")
elif clist[0] == "tol" or clist[0] == "tolerance":
try:
g.set_goal_tolerance(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'")
elif clist[0] == "time":
try:
g.set_planning_time(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'")
elif clist[0] == "constrain":
try:
g.set_path_constraints(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
if self._db_host != None:
return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.")
else:
return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
elif clist[0] == "wait":
try:
time.sleep(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
except:
return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], self._db_port)
self._db_host = clist[1]
return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
except:
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
if len(clist) == 3:
if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]):
self._last_plan = None
try:
offset = float(clist[2])
(axis, factor) = self.GO_DIRS[clist[1]]
return self.command_go_offset(g, offset, factor, axis, clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'")
elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
g.allow_looking(True)
else:
g.allow_looking(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"):
if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
g.allow_replanning(True)
else:
g.allow_replanning(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], int(clist[2]))
self._db_host = clist[1]
self._db_port = int(clist[2])
return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
except:
self._db_host = None
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'")
if len(clist) == 4:
if clist[0] == "rotate":
try:
g.set_rpy_target([float(x) for x in clist[1:]])
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation values '" + " ".join(clist[1:]) + "'")
if len(clist) >= 7:
if clist[0] == "go":
self._last_plan = None
approx = False
if (len(clist) > 7):
if (clist[7] == "approx" or clist[7] == "approximate"):
approx = True
try:
p = Pose()
p.position.x = float(clist[1])
p.position.y = float(clist[2])
p.position.z = float(clist[3])
q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6]))
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
if approx:
g.set_joint_value_target(p, True)
else:
g.set_pose_target(p)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)))
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'")
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
def command_show(self, g):
known = g.get_remembered_joint_values()
res = []
for k in known.keys():
res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
return (MoveGroupInfoLevel.INFO, "\n".join(res))
def command_current(self, g):
res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
if len(g.get_end_effector_link()) > 0:
res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
return (MoveGroupInfoLevel.INFO, res)
def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
if g.has_end_effector_link():
try:
g.shift_pose_target(dimension_index, factor * offset)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
else:
return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)
def resolve_command_alias(self, cmdorig):
cmd = cmdorig.lower()
if cmd == "which":
return "id"
if cmd == "groups":
return "use"
return cmdorig
def get_help(self):
res = []
res.append("Known commands:")
res.append(" help show this screen")
res.append(" id|which display the name of the group that is operated on")
res.append(" load [<file>] load a set of interpreted commands from a file")
res.append(" save [<file>] save the currently known variables as a set of commands")
res.append(" use <name> switch to using the group named <name> (and load it if necessary)")
res.append(" use|groups show the group names that are already loaded")
res.append(" vars display the names of the known states")
res.append(" show display the names and values of the known states")
res.append(" show <name> display the value of a state")
res.append(" record <name> record the current joint values under the name <name>")
res.append(" delete <name> forget the joint values under the name <name>")
res.append(" current show the current state of the active group")
res.append(" constrain <name> use the constraint <name> as a path constraint")
res.append(" constrain clear path constraints")
res.append(" eef print the name of the end effector attached to the current group")
res.append(" go <name> plan and execute a motion to the state <name>")
res.append(" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
res.append(" go rand plan and execute a motion to a random state")
res.append(" plan <name> plan a motion to the state <name>")
res.append(" execute execute a previously computed motion plan")
res.append(" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
res.append(" tolerance show the tolerance for reaching the goal region")
res.append(" tolerance <val> set the tolerance for reaching the goal region")
res.append(" wait <dt> sleep for <dt> seconds")
res.append(" x = y assign the value of y to x")
res.append(" x[idx] = val assign a value to dimension idx of x")
res.append(" x = [v1 v2...] assign a vector of values to x")
res.append(" trace <on|off> enable/disable replanning or looking around")
res.append(" ground add a ground plane to the planning scene")
res.append(" allow replanning <true|false> enable/disable replanning")
res.append(" allow looking <true|false> enable/disable looking around")
return "\n".join(res)
def get_keywords(self):
known_vars = []
known_constr = []
if self.get_active_group() != None:
known_vars = self.get_active_group().get_remembered_joint_values().keys()
known_constr = self.get_active_group().get_known_constraints()
groups = self._robot.get_group_names()
return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars,
'help':[],
'record':known_vars,
'show':known_vars,
'wait':[],
'delete':known_vars,
'database': [],
'current':[],
'use':groups,
'load':[],
'save':[],
'pick':[],
'place':[],
'plan':known_vars,
'allow':['replanning', 'looking'],
'constrain':known_constr,
'vars':[],
'joints':[],
'tolerance':[],
'time':[],
'eef':[],
'execute':[],
'ground':[],
'id':[]}
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Ioan Sucan
import sys
import rospy
from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown
from moveit_msgs.msg import RobotState
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
robot = RobotCommander()
rospy.sleep(1)
print "Current state:"
print robot.get_current_state()
# plan to a random location
a = robot.right_arm
a.set_start_state(RobotState())
r = a.get_random_joint_values()
print "Planning to random joint position: "
print r
p = a.plan(r)
print "Solution:"
print p
class PigeonPickAndPlace:
#Class constructor (parecido com java, inicializa o que foi instanciado)
def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Pick object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
def __del__(self):
# Clean the scene
self._scene.remove_world_object(self._grasp_object_name)
def _add_object_grasp(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame() #pose
p.header.stamp = rospy.Time.now()
rospy.loginfo(self._robot.get_planning_frame())
p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01
p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01
p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0
#p.pose.orientation.x = 0.0028925
#p.pose.orientation.y = -0.0019311
#p.pose.orientation.z = -0.71058
#p.pose.orientation.w = 0.70361
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/,
# using the measure tape tool from meshlab.
# The box is the bounding box of the lego cylinder.
# The values are taken from the cylinder base diameter and height.
# the size is length (x),thickness(y),height(z)
# I don't know if the detector return this values of object
self._scene.add_box(name, p, (0.032, 0.016, 0.020))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
#rospy.logerr('Generated: %f grasps:' % grasps.size)
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
#goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 5.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 10
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_object_grasp, 0.016)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown, MoveGroupCommander
from geometry_msgs.msg import PoseStamped
import moveit_msgs.msg
import geometry_msgs.msg
import moveit_commander
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('move_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
group = MoveGroupCommander("manipulator")
# specify the planner
group.set_planner_id("RRTkConfigDefault")
rospy.sleep(3)
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
# display_trajectory_publisher = rospy.Publisher(
# '/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory)
print ">>>>> remove scenes"
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
#right_arm.set_planner_id("KPIECEkConfigDefault");
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("right_gripper_open")
#right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.7
p.pose.position.y = -0.2
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.47636
start_pose.pose.position.y = -0.21886
start_pose.pose.position.z = 0.9
start_pose.pose.orientation.x = 0.00080331
start_pose.pose.orientation.y = 0.001589
start_pose.pose.orientation.z = -2.4165e-06
start_pose.pose.orientation.w = 1
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.3)
rospy.spin()
roscpp_shutdown()
class trajectoryConstructor():
def __init__(self):
rospy.loginfo("Waiting for service " + IK_SERVICE_NAME)
rospy.wait_for_service(IK_SERVICE_NAME)
self.ik_serv = rospy.ServiceProxy(IK_SERVICE_NAME, GetPositionIK)
self.joint_list = ['torso_1_joint', 'torso_2_joint',
'head_1_joint', 'head_2_joint',
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint',
'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.left_arm = ['arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint']
self.right_arm = ['arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.right_arm_torso = ['torso_1_joint', 'torso_2_joint',
'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint',
'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint',
'arm_right_7_joint']
self.left_arm_torso = ['torso_1_joint', 'torso_2_joint',
'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint',
'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint',
'arm_left_7_joint']
# # Store answer so we ask only one time
# self.joint_state = JointState()
# self.joint_state.name = gksi_answer.kinematic_solver_info.joint_names
# self.joint_state.position = [0.0] * len(gksi_answer.kinematic_solver_info.joint_names)
# self.ik_link_name = gksi_answer.kinematic_solver_info.link_names[0]
self.r_commander = RobotCommander()
self.initial_robot_state = self.r_commander.get_current_state()
if DEBUG_MODE:
self.pub_ok_markers = rospy.Publisher('ik_ok_marker_list', MarkerArray, latch=True)
self.ok_markers = MarkerArray()
self.pub_fail_markers = rospy.Publisher('ik_fail_marker_list', MarkerArray, latch=True)
self.fail_markers = MarkerArray()
self.markers_id = 5
def getIkPose(self, pose, group="right_arm", previous_state=None):
"""Get IK of the pose specified, for the group specified, optionally using
the robot_state of previous_state (if not, current robot state will be requested) """
# point point to test if there is ik
# returns the answer of the service
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = group
rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now())
rqst.ik_request.pose_stamped.header.frame_id = 'base_link'
# Set point to check IK for
rqst.ik_request.pose_stamped.pose.position = pose.position
rqst.ik_request.pose_stamped.pose.orientation = pose.orientation
if previous_state == None:
cs = self.r_commander.get_current_state()
rqst.ik_request.robot_state = cs
else:
rqst.ik_request.robot_state = previous_state
ik_answer = GetPositionIKResponse()
if DEBUG_MODE:
timeStart = rospy.Time.now()
ik_answer = self.ik_serv.call(rqst)
if DEBUG_MODE:
durationCall= rospy.Time.now() - timeStart
rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s")
return ik_answer
def computeJointTrajFromCartesian(self, points, arm="right_arm_torso"):
#fjt_goal = FollowJointTrajectoryGoal()
poselist = []
for point in points:
qt = quaternion_from_euler(point.positions[3], point.positions[4], point.positions[5])
pose = Pose(Point(point.positions[0], point.positions[1], point.positions[2]),
Quaternion(*qt.tolist()))
poselist.append(pose)
fjt_goal = self.computeIKsPose(poselist, arm)
return fjt_goal
def computeIKsPose(self, poselist, arm="right_arm"):
rospy.loginfo("Computing " + str(len(poselist)) + " IKs" )
fjt_goal = FollowJointTrajectoryGoal()
if arm == 'right_arm_torso':
fjt_goal.trajectory.joint_names = self.right_arm_torso
elif arm == 'left_arm_torso':
fjt_goal.trajectory.joint_names = self.left_arm_torso
elif arm == 'right_arm':
fjt_goal.trajectory.joint_names = self.right_arm
elif arm == 'left_arm':
fjt_goal.trajectory.joint_names = self.left_arm
# TODO: add other options
ik_answer = None
for pose in poselist:
if ik_answer != None:
ik_answer = self.getIkPose(pose,"right_arm", previous_state=ik_answer.solution)
else:
ik_answer = self.getIkPose(pose)
if DEBUG_MODE:
rospy.loginfo("Got error_code: " + str(ik_answer.error_code.val) + " which means: " + moveit_error_dict[ik_answer.error_code.val])
if moveit_error_dict[ik_answer.error_code.val] == 'SUCCESS':
if DEBUG_MODE:
arrow = self.createArrowMarker(pose, ColorRGBA(0,1,0,1))
self.ok_markers.markers.append(arrow)
jtp = JointTrajectoryPoint()
#ik_answer = GetConstraintAwarePositionIKResponse()
# sort positions and add only the ones of the joints we are interested in
positions = self.sortOutJointList(fjt_goal.trajectory.joint_names, ik_answer.solution.joint_state)
jtp.positions = positions
# TODO: add velocities | WILL BE DONE OUTSIDE
# TODO: add acc? | DUNNO
fjt_goal.trajectory.points.append(jtp)
if DEBUG_MODE:
self.pub_ok_markers.publish(self.ok_markers)
else:
if DEBUG_MODE:
arrow = self.createArrowMarker(pose, ColorRGBA(1,0,0,1))
self.fail_markers.markers.append(arrow)
self.pub_fail_markers.publish(self.fail_markers)
return fjt_goal
def sortOutJointList(self, joint_name_list, joint_state):
""" Get only the joints we are interested in and it's values and return it in
joint_state.name and joint_state.points format"""
if DEBUG_MODE:
rospy.loginfo("Sorting jointlist...")
list_to_iterate = joint_name_list
curr_j_s = joint_state
ids_list = []
position_list = []
for joint in list_to_iterate:
idx_in_message = curr_j_s.name.index(joint)
ids_list.append(idx_in_message)
position_list.append(curr_j_s.position[idx_in_message])
return position_list
def adaptTimesAndVelocitiesOfMsg(self, trajectory, plan, desired_final_time):
"""Adapt the times and velocities of the message for the controller
from the times computed in the DMP and velocities 0.0, controller will take care of it"""
rospy.loginfo("Adapting times and velocities...")
traj = trajectory #FollowJointTrajectoryGoal()
p = plan #GetDMPPlanResponse()
# we should have the same number of points on each, NOPE, as we can have points that IK fails
# if len(traj.trajectory.points) != len(p.plan.points):
# rospy.logerr("Oops, something went wrong, different number of points")
# rospy.logerr("generated trajectory: " + str(len(traj.trajectory.points)) + " plan: " + str(len(p.plan.points)))
# exit(0)
point = JointTrajectoryPoint()
counter = 0
for point in traj.trajectory.points:
#rospy.loginfo("Step " + str(counter) + " / " + str(len(traj.trajectory.points)))
counter += 1
point.velocities.extend(len(point.positions) * [0.0]) # adding 0.0 as speeds
point.time_from_start = rospy.Duration( counter * (desired_final_time / len(traj.trajectory.points)) )
return traj
def publish_markers(self):
while True:
self.pub_ok_markers.publish(self.ok_markers)
self.pub_fail_markers.publish(self.fail_markers)
rospy.sleep(0.1)
def createArrowMarker(self, pose, color):
marker = Marker()
marker.header.frame_id = 'base_link'
marker.type = marker.ARROW
marker.action = marker.ADD
general_scale = 0.01
marker.scale.x = general_scale
marker.scale.y = general_scale / 3.0
marker.scale.z = general_scale / 10.0
marker.color = color
marker.pose.orientation = pose.orientation
marker.pose.position = pose.position
marker.id = self.markers_id
self.markers_id += 1
return marker
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)
# clean the scene
#scene.remove_world_object("block1")
#scene.remove_world_object("block2")
#scene.remove_world_object("block3")
#scene.remove_world_object("block4")
#scene.remove_world_object("table")
#scene.remove_world_object("bowl")
#scene.remove_world_object("box")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = -0.4
p.pose.position.z = 0.85
p.pose.orientation.w = 1.57
scene.add_box("block1", p, (0.044, 0.044, 0.044))
p.pose.position.y = -0.2
p.pose.position.z = 0.175
import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
import tf
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("arm")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Robot Links for arm:"
print robot.get_link_names("arm")
print "============ Robot Links for gripper:"
print robot.get_link_names("gripper")
print right_arm.get_end_effector_link()
print "============ Printing robot state"
#print robot.get_current_state()
print "============"
rospy.sleep(1)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
rf = robot.get_planning_frame()
print rf
open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.087, 100))
close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.03, -1))
self.ac.send_goal(open)
self.ac.wait_for_result()
#print self.ac.get_result()
# Prepare Gazebo Subscriber
self.pwh = None
self.bl = ['ground_plane','pr2']
self.pa = []
self.idx = []
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# Give the scene a chance to catch up
rospy.sleep(2)
while self.pwh is None:
rospy.sleep(0.05)
#self.new = [ x for x in self.pwh.name if x not in self.bl ]
#for i in self.new:
#self.idx.append(self.pwh.name.index(i))
#for j in self.idx:
#self.pa.append(self.pwh.pose[j])
##print self.pa
#self.pose_array = PoseArray()
#self.pose_array.header.frame_id = REFERENCE_FRAME
#self.pose_array.poses = self.pa
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.019
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 0.995
scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
#Publish target frame
#self.object_frames_pub.publish(target_pose)
# pick an object
target_pose.pose.position.z += 0.15
robot.right_arm.pick(target_id)
self.ac.send_goal(close)
self.ac.wait_for_result()
#print self.ac.get_result()
rospy.sleep(2)
def setUpClass(self):
self.commander = RobotCommander("%srobot_description" % self.PLANNING_NS, self.PLANNING_NS)
self.group = self.commander.get_group(self.PLANNING_GROUP)
from hermes_virtual_robot.srv import *
#http://moveit.ros.org/doxygen/annotated.html
#https://github.com/ros-planning/moveit_commander
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
rospy.wait_for_service('grasp_hand')
scene = PlanningSceneInterface()
robot = RobotCommander()
pose = PoseStamped()
rospy.sleep(1)
# clean the scene
scene.remove_world_object("grasp_point")
scene.remove_world_object("right_shoe")
# move_group
group_right = MoveGroupCommander("r_arm")
group_left = MoveGroupCommander("l_arm")
#Get Current state
state = robot.get_current_variable_values()
# modify the pose
p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
# add the cup
scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
# modify the pose
p.pose.position.x = 0.72
p.pose.position.z = 0.05
# add the pen
scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
rospy.sleep(1)
# print the existing groups
robot = RobotCommander()
print "Available groups: ",robot.get_group_names()
# setup the arm group and its planner
arm = MoveGroupCommander("arm")
arm.set_start_state_to_current_state()
arm.set_planner_id("RRTstarkConfigDefault")
arm.set_planning_time(5.0)
arm.detach_object("pen")
# set the arm to a safe target
arm.set_named_target("gamma")
# plan and execute the motion
arm.go()
# setup the hand group and its planner
