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)
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)
eef = MoveGroupCommander("endeffector")
rospy.sleep(1)
#Start State Gripper
group_variable_values = eef.get_current_joint_values()
group_variable_values[0] = 0.0
eef.set_joint_value_target(group_variable_values)
plan2 = eef.plan()
arm.execute(plan2)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
k = PoseStamped()
k.header.frame_id = robot.get_planning_frame()
k.pose.position.x = 0.0
k.pose.position.y = 0.0
k.pose.position.z = -0.05
scene.add_box("table", k, (2, 2, 0.0001))
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.35
p.pose.position.y = 0.35
p.pose.position.z = 0.05
scene.add_box("part", p, (0.09, 0.09, 0.09))
rospy.sleep(1)
#Planning Part 1: Move to correct xy coorinate
temPose = p
print temPose
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_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
#right_arm.set_goal_position_tolerance(0.005)
right_arm.set_goal_orientation_tolerance(0.005)
right_arm.set_planning_time(5)
right_arm.set_num_planning_attempts(5)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
#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("default")
right_arm.go(wait=True)
right_gripper.set_named_target("open")
right_gripper.go(wait=True)
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add an object to be grasped
p.pose.position.x = 0.170
p.pose.position.y = 0.04
p.pose.position.z = 0.3
#scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
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.00756585784256
start_pose.pose.position.y = -0.225419849157
start_pose.pose.position.z = 0.117192693055
start_pose.pose.orientation.x = 0.95493721962
start_pose.pose.orientation.y = -0.0160209629685
start_pose.pose.orientation.z = -0.00497157918289
start_pose.pose.orientation.w = 0.296333402395
print("going to pick up pose")
right_arm.set_pose_target(start_pose)
right_gripper.set_named_target("close")
right_arm.go(wait=True)
right_gripper.go(wait=True)
rospy.sleep(1)
right_arm.set_named_target("default")
right_arm.go(wait=True)
next_pose = PoseStamped()
next_pose.header.frame_id = FIXED_FRAME
next_pose.pose.position.x = -0.100732862949
next_pose.pose.position.y = -0.210876911879
next_pose.pose.position.z = 0.244678631425
next_pose.pose.orientation.x = 0.784905433655
next_pose.pose.orientation.y = -0.177844554186
next_pose.pose.orientation.z = -0.131161093712
next_pose.pose.orientation.w = 0.578870952129
right_arm.set_pose_target(next_pose)
right_gripper.set_named_target("open")
right_arm.go(wait=True)
right_gripper.go(wait=True)
rospy.sleep(3)
right_arm.set_named_target("default")
right_arm.go(wait=True)
rospy.spin()
roscpp_shutdown()
class TestPlanners(object):
def __init__(self, group_id, planner):
rospy.init_node('moveit_test_planners', anonymous=True)
self.pass_list = []
self.fail_list = []
self.planner = planner
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(group_id)
rospy.sleep(1)
self.group.set_planner_id(self.planner)
self.test_trajectories_empty_environment()
self.test_waypoints()
self.test_trajectories_with_walls_and_ground()
for pass_test in self.pass_list:
print(pass_test)
for fail_test in self.fail_list:
print(fail_test)
def _add_walls_and_ground(self):
# publish a scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below ground (to prevent collision with
# the robot itself)
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.1))
p.pose.position.x = 0.4
p.pose.position.y = 0.85
p.pose.position.z = 0.4
p.pose.orientation.x = 0.5
p.pose.orientation.y = -0.5
p.pose.orientation.z = 0.5
p.pose.orientation.w = 0.5
self.scene.add_box("wall_front", p, (0.8, 2, 0.01))
p.pose.position.x = 1.33
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707388
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.706825
self.scene.add_box("wall_right", p, (0.8, 2, 0.01))
p.pose.position.x = -0.5
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707107
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.707107
self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
# rospy.sleep(1)
def _check_plan(self, plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
def _plan_joints(self, joints):
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
return self._check_plan(plan)
def test_trajectories_rotating_each_joint(self):
# test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
test_joint_values = [numpy.pi / 2.0]
joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
# Joint 4th is colliding with the hand
# for joint in range(6):
for joint in [0, 1, 2, 3, 5]:
for value in test_joint_values:
joints[joint] = value
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
def test_trajectories_empty_environment(self):
# Up - Does not work with sbpl but it does with ompl
joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# All joints up
joints = [
-1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Down
joints = [
-0.000348431194526, 0.397651011661, 0.0766181197394,
-0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# left
joints = [
0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
0.146075718452, 0.00420656698366, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Front
joints = [
1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
-1.42700242769, 3.1415450794, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Behind
joints = [
1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
0.694689321451, -0.390769365032, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Should fail because it is in self-collision
joints = [
-0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
1.04235601797, -1.69730925867, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
def test_waypoints(self):
# Start planning in a given joint position
joints = [
-0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
0.344227622185, -3.03250264451, 0.0, 0.0
]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:8] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
waypoints = []
initial_pose = self.group.get_current_pose().pose
initial_pose.position.x = -0.301185959729
initial_pose.position.y = 0.517069787724
initial_pose.position.z = 1.20681710541
initial_pose.orientation.x = 0.0207499700474
initial_pose.orientation.y = -0.723943002716
initial_pose.orientation.z = -0.689528413407
initial_pose.orientation.w = 0.00515118111221
# start with a specific position
waypoints.append(initial_pose)
# first move it down
wpose = geometry_msgs.msg.Pose()
wpose.orientation = waypoints[0].orientation
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z - 0.20
waypoints.append(copy.deepcopy(wpose))
# second front
wpose.position.y += 0.20
waypoints.append(copy.deepcopy(wpose))
# third side
wpose.position.x -= 0.20
waypoints.append(copy.deepcopy(wpose))
# fourth return to initial pose
wpose = waypoints[0]
waypoints.append(copy.deepcopy(wpose))
(plan3,
fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
if not self._check_plan(plan3) and fraction > 0.8:
self.fail_list.append("Failed: test_waypoints, " + self.planner)
else:
self.pass_list.append("Passed: test_waypoints, " + self.planner)
def test_trajectories_with_walls_and_ground(self):
self._add_walls_and_ground()
# Should fail to plan: Goal is in collision with the wall_front
joints = [
0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
0.302143499777, 0.00130280337897, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Should fail to plan: Goal is in collision with the ground
joints = [
3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
0.000133883149666, -0.594498939239, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Goal close to right corner
joints = [
0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
-1.3516255551, 2.8225061435, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed, test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
self.scene.remove_world_object("ground")
self.scene.remove_world_object("wall_left")
self.scene.remove_world_object("wall_right")
self.scene.remove_world_object("wall_front")
def init():
global marker_array_pub, marker_pub, tf_broadcaster, tf_listener
global move_group, turntable
global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir
rospy.init_node('acquisition')
camera_mesh = rospy.get_param('~camera_mesh', None)
camera_orientation = rospy.get_param('~camera_orientation', None)
camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0])
camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1])
min_distance = rospy.get_param('~min_distance', 0.2)
max_distance = rospy.get_param('~max_distance', min_distance)
max_velocity = rospy.get_param('~max_velocity', 0.1)
num_positions = rospy.get_param('~num_positions', 15)
num_spins = rospy.get_param('~num_spins', 8)
object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2]))
photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0])
photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0])
ptpip = rospy.get_param('~ptpip', None)
reach = rospy.get_param('~reach', 0.85)
simulation = '/gazebo' in get_node_names()
test = rospy.get_param('~test', True)
turntable_pos = rospy.get_param(
'~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05])
turntable_radius = rospy.get_param('~turntable_radius', 0.2)
wall_thickness = rospy.get_param('~wall_thickness', 0.04)
marker_array_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=1,
latch=True)
marker_pub = rospy.Publisher('visualization_marker',
Marker,
queue_size=1,
latch=True)
tf_broadcaster = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
move_group = MoveGroupCommander('manipulator')
move_group.set_max_acceleration_scaling_factor(
1.0 if simulation else max_velocity)
move_group.set_max_velocity_scaling_factor(
1.0 if simulation else max_velocity)
robot = RobotCommander()
scene = PlanningSceneInterface(synchronous=True)
try:
st_control = load_source(
'st_control',
os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts',
'iai_scanning_table', 'st_control.py'))
turntable = st_control.ElmoUdp()
if turntable.check_device():
turntable.configure()
turntable.reset_encoder()
turntable.start_controller()
turntable.set_speed_deg(30)
except Exception as e:
print(e)
sys.exit('Could not connect to turntable.')
if simulation:
move_home()
elif not test:
working_dir = rospy.get_param('~working_dir', None)
if working_dir is None:
sys.exit('Working directory not specified.')
elif not camera.init(
os.path.join(os.path.dirname(os.path.abspath(__file__)), '..',
'out', working_dir, 'images'), ptpip):
sys.exit('Could not initialize camera.')
# add ground plane
ps = PoseStamped()
ps.header.frame_id = robot.get_planning_frame()
scene.add_plane('ground', ps)
# add photobox
ps.pose.position.x = photobox_pos[
0] + photobox_size[0] / 2 + wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_left', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[
0] - photobox_size[0] / 2 - wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_right', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[
1] - photobox_size[1] / 2 - wall_thickness / 2
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_back', ps,
(photobox_size[0], wall_thickness, photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] - wall_thickness / 2
scene.add_box('box_ground', ps,
(photobox_size[0], photobox_size[1], wall_thickness))
# add turntable
turntable_height = turntable_pos[2] - photobox_pos[2]
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = photobox_pos[2] + turntable_height / 2
scene.add_cylinder('turntable', ps, turntable_height, turntable_radius)
# add object on turntable
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = turntable_pos[2] + object_size[2] / 2
scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2)
# add cable mounts
scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount')
scene.remove_attached_object('forearm_link', 'forearm_cable_mount')
scene.remove_world_object('upper_arm_cable_mount')
scene.remove_world_object('forearm_cable_mount')
if ptpip is None and not simulation:
size = [0.08, 0.08, 0.08]
ps.header.frame_id = 'upper_arm_link'
ps.pose.position.x = -0.13
ps.pose.position.y = -0.095
ps.pose.position.z = 0.135
scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size)
ps.header.frame_id = 'forearm_link'
ps.pose.position.x = -0.275
ps.pose.position.y = -0.08
ps.pose.position.z = 0.02
scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size)
# add camera
eef_link = move_group.get_end_effector_link()
ps = PoseStamped()
ps.header.frame_id = eef_link
scene.remove_attached_object(eef_link, 'camera_0')
scene.remove_attached_object(eef_link, 'camera_1')
scene.remove_world_object('camera_0')
scene.remove_world_object('camera_1')
ps.pose.position.y = camera_pos[1]
ps.pose.position.z = camera_pos[2]
if camera_mesh:
ps.pose.position.x = camera_pos[0]
quaternion = tf.transformations.quaternion_from_euler(
math.radians(camera_orientation[0]),
math.radians(camera_orientation[1]),
math.radians(camera_orientation[2]))
ps.pose.orientation.x = quaternion[0]
ps.pose.orientation.y = quaternion[1]
ps.pose.orientation.z = quaternion[2]
ps.pose.orientation.w = quaternion[3]
scene.attach_mesh(eef_link, 'camera_0', ps,
os.path.expanduser(camera_mesh), camera_size)
if not simulation:
scene.attach_mesh(eef_link, 'camera_1', ps,
os.path.expanduser(camera_mesh),
numpy.array(camera_size) * 1.5)
vertices = scene.get_attached_objects(
['camera_0'])['camera_0'].object.meshes[0].vertices
camera_size[0] = max(vertice.x for vertice in vertices) - min(
vertice.x for vertice in vertices)
camera_size[1] = max(vertice.y for vertice in vertices) - min(
vertice.y for vertice in vertices)
camera_size[2] = max(vertice.z for vertice in vertices) - min(
vertice.z for vertice in vertices)
else:
ps.pose.position.x = camera_pos[0] + camera_size[0] / 2
scene.attach_box(eef_link, 'camera_0', ps, camera_size)
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 add_box:
def __init__(self):
# Retrieve params:
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)
# Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景：
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 __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._grasp_object2_name)
#----------------------------------1hang---------------------------------
def _add_grasp_block_(self, name):
"""
Create and add block to the scene创建场景并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.6
p.pose.position.y = -0.0
p.pose.position.z = 0.75
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.045, 0.045, 0.045))
return p.pose
def _add_grasp_fanfkuai_(self, name):
"""
Create and add block to the scene创建场景并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.6
p.pose.position.y = 0.4
p.pose.position.z = 0.75
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.045, 0.045, 0.045))
return p.pose
class WarehousePlanner(object):
def __init__(self):
rospy.init_node('moveit_warehouse_planner', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(2)
group_id = rospy.get_param("~arm_group_name")
self.eef_step = rospy.get_param("~eef_step", 0.01)
self.jump_threshold = rospy.get_param("~jump_threshold", 1000)
self.group = MoveGroupCommander(group_id)
# self._add_ground()
self._robot_name = self.robot._r.get_robot_name()
self._robot_link = self.group.get_end_effector_link()
self._robot_frame = self.group.get_pose_reference_frame()
self._min_wp_fraction = rospy.get_param("~min_waypoint_fraction", 0.9)
rospy.sleep(4)
rospy.loginfo("Waiting for warehouse services...")
rospy.wait_for_service('/list_robot_states')
rospy.wait_for_service('/get_robot_state')
rospy.wait_for_service('/has_robot_state')
rospy.wait_for_service('/compute_fk')
self._list_states = rospy.ServiceProxy('/list_robot_states',
ListStates)
self._get_state = rospy.ServiceProxy('/get_robot_state', GetState)
self._has_state = rospy.ServiceProxy('/has_robot_state', HasState)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
rospy.loginfo("Service proxies connected")
self._tr_frm_list_srv = rospy.Service('plan_trajectory_from_list',
PlanTrajectoryFromList,
self._plan_from_list_cb)
self._tr_frm_prfx_srv = rospy.Service('plan_trajectory_from_prefix',
PlanTrajectoryFromPrefix,
self._plan_from_prefix_cb)
self._execute_tr_srv = rospy.Service('execute_planned_trajectory',
ExecutePlannedTrajectory,
self._execute_plan_cb)
self.__plan = None
def get_waypoint_names_by_prefix(self, prefix):
regex = "^" + str(prefix) + ".*"
waypoint_names = self._list_states(regex, self._robot_name).states
return waypoint_names
def get_pose_from_state(self, state):
header = Header()
fk_link_names = [self._robot_link]
header.frame_id = self._robot_frame
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0].pose
def get_cartesian_waypoints(self, waypoint_names):
waypoints = []
waypoints.append(self.group.get_current_pose().pose)
for name in waypoint_names:
if self._has_state(name, self._robot_name).exists:
robot_state = self._get_state(name, "").state
waypoints.append(self.get_pose_from_state(robot_state))
else:
rospy.logerr("Waypoint %s doesn't exist for robot %s.", name,
self._robot_name)
return waypoints
def _add_ground(self):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below the dome
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.01))
rospy.sleep(1)
def plan_from_filter(self, prefix):
waypoint_names = self.get_waypoint_names_by_prefix(prefix)
waypoint_names.sort()
if 0 == len(waypoint_names):
rospy.logerr(
"No waypoints found for robot %s with prefix %s. " +
"Can't make trajectory :(", self._robot_name, str(prefix))
return False
rospy.loginfo(
"Creating trajectory with %d waypoints selected by " +
"prefix %s.", len(waypoint_names), str(prefix))
return self.plan_from_list(waypoint_names)
def plan_from_list(self, waypoint_names):
self.group.clear_pose_targets()
waypoints = self.get_cartesian_waypoints(waypoint_names)
if len(waypoints) != len(waypoint_names) + 1:
# +1 because current position is used as first waypiont.
rospy.logerr("Not all waypoints existed, not executing.")
return False
(plan,
fraction) = self.group.compute_cartesian_path(waypoints,
self.eef_step,
self.jump_threshold)
if fraction < self._min_wp_fraction:
rospy.logerr(
"Only managed to generate trajectory through" +
"%f of waypoints. Not executing", fraction)
return False
self.__plan = plan
return True
def _plan_from_list_cb(self, request):
# check all exist
self.__plan = None
rospy.loginfo("Creating trajectory from points given: %s",
",".join(request.waypoint_names))
return self.plan_from_list(request.waypoint_names)
def _plan_from_prefix_cb(self, request):
self.__plan = None
rospy.loginfo("Creating trajectory from points filtered by prefix %s",
request.prefix)
return self.plan_from_filter(request.prefix)
def _execute_plan_cb(self, request):
if self.__plan is None:
rospy.logerr("No plan stored!")
return False
rospy.loginfo("Executing stored plan")
response = self.group.execute(self.__plan)
self.__plan = None
return response
def main():
rospy.init_node('Baxter_Env')
robot = RobotCommander()
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=0)
rospy.sleep(2)
# centre table
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 1. # position of the table in the world frame
p1.pose.position.y = 0.
p1.pose.position.z = 0.
# left table (from baxter's perspective)
p1_l = PoseStamped()
p1_l.header.frame_id = robot.get_planning_frame()
p1_l.pose.position.x = 0.5 # position of the table in the world frame
p1_l.pose.position.y = 1.
p1_l.pose.position.z = 0.
p1_l.pose.orientation.x = 0.707
p1_l.pose.orientation.y = 0.707
# right table (from baxter's perspective)
p1_r = PoseStamped()
p1_r.header.frame_id = robot.get_planning_frame()
p1_r.pose.position.x = 0.5 # position of the table in the world frame
p1_r.pose.position.y = -1.
p1_r.pose.position.z = 0.
p1_r.pose.orientation.x = 0.707
p1_r.pose.orientation.y = 0.707
# open back shelf
p2 = PoseStamped()
p2.header.frame_id = robot.get_planning_frame()
p2.pose.position.x = 1.2 # position of the table in the world frame
p2.pose.position.y = 0.0
p2.pose.position.z = 0.75
p2.pose.orientation.x = 0.5
p2.pose.orientation.y = -0.5
p2.pose.orientation.z = -0.5
p2.pose.orientation.w = 0.5
pw = PoseStamped()
pw.header.frame_id = robot.get_planning_frame()
# add an object to be grasped
p_ob1 = PoseStamped()
p_ob1.header.frame_id = robot.get_planning_frame()
p_ob1.pose.position.x = .92
p_ob1.pose.position.y = 0.3
p_ob1.pose.position.z = 0.89
# the ole duck
p_ob2 = PoseStamped()
p_ob2.header.frame_id = robot.get_planning_frame()
p_ob2.pose.position.x = 0.87
p_ob2.pose.position.y = -0.4
p_ob2.pose.position.z = 0.24
# clean environment
scene.remove_world_object("table_center")
scene.remove_world_object("table_side_left")
scene.remove_world_object("table_side_right")
scene.remove_world_object("shelf")
scene.remove_world_object("wall")
scene.remove_world_object("part")
scene.remove_world_object("duck")
rospy.sleep(1)
scene.add_box("table_center", p1,
size=(.5, 1.5, 0.4)) # dimensions of the table
scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4))
scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4))
scene.add_mesh("shelf", p2, "./bookshelf_light.stl", size=(.025, .01, .01))
scene.add_plane("wall", pw, normal=(0, 1, 0))
part_size = (0.07, 0.05, 0.12)
scene.add_box("part", p_ob1, size=part_size)
scene.add_mesh("duck", p_ob2, "./duck.stl", size=(.001, .001, .001))
rospy.sleep(1)
print(scene.get_known_object_names())
## ---> SET COLOURS
table_color = color_norm([105, 105,
105]) # normalize colors to range [0, 1]
shelf_color = color_norm([139, 69, 19])
duck_color = color_norm([255, 255, 0])
_colors = {}
setColor('table_center', _colors, table_color, 1)
setColor('table_side_left', _colors, table_color, 1)
setColor('table_side_right', _colors, table_color, 1)
setColor('shelf', _colors, shelf_color, 1)
setColor('duck', _colors, duck_color, 1)
sendColors(_colors, scene)
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()
# scene.add_box("table", p, (0.5, 1.5, 0.7))
rospy.sleep(1)
#bring the arm near the object
(aim_x, aim_y, aim_z,
aim_yaw) = onine_arm.get_valid_pose(item_translation[0],
item_translation[1],
item_translation[2], -0.20)
# onine_arm.ready()
# onine_arm.open_gripper()
# onine_arm.go(aim_x, aim_y, aim_z, aim_yaw)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = item_translation[0]
p.pose.position.y = item_translation[1]
p.pose.position.z = item_translation[2]
scene.add_box("target", p, (0.01, 0.01, 0.08))
# rospy.sleep(20)
# (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], - 0.080)
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "left_gripper_link"
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.position.x = aim_x
grasp_pose.pose.position.y = aim_y
grasp_pose.pose.position.z = aim_z
grasp_pose.pose.orientation = Quaternion(
class planning:
def __init__(self):
# Retrieve params:
# if there is no param named 'table_object_name' then use the default
# set the scene in rzvi
# original initilization is for 2
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table') # table
self._grasp_object_name_1 = rospy.get_param('~grasp_object_name',
'Grasp_Object1') # box1
self._grasp_object_name_2 = rospy.get_param(
'~grasp_object_name2', 'Wall_behind') # wall_behind as a obstacle
self._grasp_object_name_3 = rospy.get_param(
'~grasp_object_name3', 'Wall_above') # wall_above as a obstacle
self._grasp_object_name_screw = rospy.get_param(
'~grasp_object_name5', 'screw') # screw
#print self._grasp_object_name_screw
self._grasp_object_name_capscrew4 = rospy.get_param(
'~grasp_object_name4', 'capscrew4') # capscrew
#print self._grasp_object_name_capscrew4
#### Not needed in the model spawn Section ####
# Create (debugging) publishers(display in rviz):
#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_1)
self._scene.remove_world_object(
self._grasp_object_name_2) #wall_behind
#self._scene.remove_world_object(self._grasp_object_name_3) #wall_above
self._scene.remove_world_object(
self._grasp_object_name_capscrew4) #capscrew4
self._scene.remove_world_object(self._grasp_object_name_screw) #screw
##############3# Add table and Coke can objects to the planning scene:#####################################
self._pose_table = self._add_table(self._table_object_name)
self._add_grasp_block_1(self._grasp_object_name_1)
# ##########################################3Adding obstacle_behind#############################
p_wall = PoseStamped()
robot = moveit_commander.RobotCommander()
p_wall.header.frame_id = robot.get_planning_frame()
p_wall.header.stamp = rospy.Time.now()
p_wall.pose.position.x = -0.25
p_wall.pose.position.y = 0.0
p_wall.pose.position.z = 1.5
q_wall_temp = quaternion_from_euler(0.0, 80.0, 0.0) #Q number
p_wall.pose.orientation = Quaternion(*q_wall_temp)
self._scene.add_box(self._grasp_object_name_2, p_wall, (3, 3, 0.05))
###################################### Adding obstacle wall_above######################################
p_wall2 = PoseStamped()
p_wall2.header.frame_id = robot.get_planning_frame()
p_wall2.header.stamp = rospy.Time.now()
p_wall2.pose.position.x = table_x
p_wall2.pose.position.y = table_y
p_wall2.pose.position.z = 1.25
q_wall2_temp = quaternion_from_euler(0.0, 0.0, 0.0) #Q number
p_wall2.pose.orientation = Quaternion(*q_wall2_temp)
#self._scene.add_box(self._grasp_object_name_3, p_wall2, (3, 3, 0.05))
###################################### Adding obstacle side-1#####################################
p_wall3 = PoseStamped()
p_wall3.header.frame_id = robot.get_planning_frame()
p_wall3.header.stamp = rospy.Time.now()
p_wall3.pose.position.x = table_x
p_wall3.pose.position.y = table_y - 0.8 / 2 - 0.2
p_wall3.pose.position.z = table_z
q_wall3_temp = quaternion_from_euler(80.0, 0.0, 0.0) #Q number
p_wall3.pose.orientation = Quaternion(*q_wall3_temp)
self._scene.add_box("wall_side-1", p_wall3, (2, 2, 0.05))
###################################### Adding obstacle side -2######################################
p_wall4 = PoseStamped()
p_wall4.header.frame_id = robot.get_planning_frame()
p_wall4.header.stamp = rospy.Time.now()
p_wall4.pose.position.x = table_x
p_wall4.pose.position.y = table_y + 0.8 / 2 + 0.05
p_wall4.pose.position.z = table_z
q_wall4_temp = quaternion_from_euler(80.0, 0.0, 0.0) #Q number
p_wall4.pose.orientation = Quaternion(*q_wall4_temp)
self._scene.add_box("wall_side-2", p_wall4, (2, 2, 0.05))
##############################################adding screw###########################
screw = PoseStamped()
screw.header.frame_id = robot.get_planning_frame()
screw.header.stamp = rospy.Time.now()
screw.pose.position.x = table_x
screw.pose.position.y = table_y - 0.2
screw.pose.position.z = box1_z + 0.02
screw_q = quaternion_from_euler(0.0, 0.0, 0.0) #Q number
screw.pose.orientation = Quaternion(*screw_q)
print self._scene.add_box(self._grasp_object_name_screw, screw,
(0.036, 0.046, 0.032))
##############################################adding capscrew 0.045###########################
capscrew4 = PoseStamped()
capscrew4.header.frame_id = robot.get_planning_frame()
capscrew4.header.stamp = rospy.Time.now()
capscrew4.pose.position.x = table_x
capscrew4.pose.position.y = table_y
capscrew4.pose.position.z = box1_z + 0.02
q4 = quaternion_from_euler(0.0, 0.0, 0.0) #Q number
capscrew4.pose.orientation = Quaternion(*q4)
print self._scene.add_box(self._grasp_object_name_capscrew4, capscrew4,
(0.05, 0.05, 0.040))
rospy.sleep(1.0)
# gripper graspping point setting.
# Define target place pose:
#########################################Section: Neccessay function for (displaying)Rviz###################################
######this section for adding stuffs in the planning scene.(table, box1, box2, etc...) in RViz############################3
def _add_table(self, name):
"""
Create and add table to the scene
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = table_x
p.pose.position.y = table_y
p.pose.position.z = table_z
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.
# display in RZvi
print self._scene.add_box(name, p,
(1, 1, 0.05)) #arg(name, pose, size)
return p.pose
def _add_grasp_block_1(self, name):
"""
Create and add block to the scene
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = box1_x + 0.3
p.pose.position.y = box1_y + 0.3
p.pose.position.z = box1_z
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.045, 0.045, 0.045))
return p.pose
class Pick_Place:
def __init__(self):
# rospy.init_node('pick_and_place')
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
jointslimit = joints_setup["joints_limit"]
home_value = joints_setup["home_value"]
j1 = home_value["joint_1"]
j2 = home_value["joint_2"]
j3 = home_value["joint_3"]
j4 = home_value["joint_4"]
j5 = home_value["joint_5"]
j6 = home_value["joint_6"]
g = home_value["gripper"]
self.set_home_value([j1, j2, j3, j4, j5, j6, g])
home_value = joints_setup["pick_place_home_value"]
j1 = home_value["joint_1"]
j2 = home_value["joint_2"]
j3 = home_value["joint_3"]
j4 = home_value["joint_4"]
j5 = home_value["joint_5"]
j6 = home_value["joint_6"]
g = home_value["gripper"]
self.set_pick_place_home_value([j1, j2, j3, j4, j5, j6, g])
self.object_list = {}
self.obstacle_list = {}
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
rospy.loginfo("Spawning Objects in Gazebo and planning scene")
objects = objects_info["objects"]
objects_name = objects.keys()
for object_name in objects_name:
name = object_name
shape = objects[name]["shape"]
color = objects[name]["color"]
x = objects[name]["pose"]["x"]
y = objects[name]["pose"]["y"]
z = objects[name]["pose"]["z"]
roll = objects[name]["pose"]["roll"]
pitch = objects[name]["pose"]["pitch"]
yaw = objects[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = x
p.pose.position.y = y
p.pose.position.z = z
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation = Quaternion(*q)
if shape == "box":
x = objects[name]["size"]["x"]
y = objects[name]["size"]["y"]
z = objects[name]["size"]["z"]
p.pose.position.z += z / 2
height = z
width = y
length = x
self.object_list[name] = Object(p.pose, p.pose, height,
width, length, shape,
color)
elif shape == "cylinder":
height = objects[name]["size"]["height"]
radius = objects[name]["size"]["radius"]
p.pose.position.z += height / 2
self.object_list[name] = Object(p.pose, p.pose, height,
radius * 2, radius * 2,
shape, color)
elif shape == "sphere":
radius = objects[name]["size"]
p.pose.position.z += radius
self.object_list[name] = Object(p.pose, p.pose, radius * 2,
radius * 2, radius * 2,
shape, color)
objects = objects_info["objects"]
obstacles = objects_info["obstacles"]
obstacles_name = obstacles.keys()
for object_name in obstacles_name:
name = object_name
x = obstacles[name]["pose"]["x"]
y = obstacles[name]["pose"]["y"]
z = obstacles[name]["pose"]["z"]
roll = obstacles[name]["pose"]["roll"]
pitch = obstacles[name]["pose"]["pitch"]
yaw = obstacles[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = x
p.pose.position.y = y
p.pose.position.z = z
x = obstacles[name]["size"]["x"]
y = obstacles[name]["size"]["y"]
z = obstacles[name]["size"]["z"]
p.pose.position.z += z / 2
height = z
width = y
length = x
self.obstacle_list[name] = Obstacle(p.pose, p.pose, height,
width, length)
# self.object_list = object_list
self.goal_list = {}
self.set_target_info()
self.gripper_width = {}
self.set_gripper_width_relationship()
self.arm = moveit_commander.MoveGroupCommander("ur10_manipulator")
self.arm.set_goal_tolerance(0.01)
self.arm.set_pose_reference_frame("ur10_base_link")
self.gripperpub = rospy.Publisher("gripper_controller/command",
JointTrajectory,
queue_size=0)
self.transform_arm_to_baselink = Point()
self.get_arm_to_baselink()
self.gripper_length = 0.33
self.get_workspace()
self.message_pub = rospy.Publisher("/gui_message",
String,
queue_size=0)
self.updatepose_pub = rospy.Publisher("/updatepose",
Bool,
queue_size=0)
self.robot_pose = Pose()
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.robot_pose_callback)
def send_message(self, message):
msg = String()
msg.data = message
self.message_pub.publish(msg)
def updatepose_trigger(self, value):
msg = Bool()
msg.data = value
self.updatepose_pub.publish(msg)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def clean_all_objects_in_scene(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
self.clean_scene(object_name)
def spawn_all_objects(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
self.spawn_object_rviz(object_name)
def spawn_object_rviz(self, object_name):
self.clean_scene(object_name)
this_object = self.object_list[object_name]
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose = copy.deepcopy(this_object.abs_pose)
robot_pose = self.get_robot_pose()
p.pose.position.x -= robot_pose.position.x
p.pose.position.y -= robot_pose.position.y
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = -euler[2]
quat = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
self.object_list[object_name].relative_pose = self.baselink2arm(p.pose)
shape = this_object.shape
if shape == "box":
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(object_name, p, size)
elif shape == "cylinder":
height = this_object.height
radius = this_object.width / 2
self.scene.add_cylinder(object_name, p, height, radius)
elif shape == "sphere":
radius = this_object.width / 2
self.scene.add_sphere(object_name, p, radius)
rospy.sleep(0.5)
def spawn_obstacle_rviz(self, obstacle_name):
self.clean_scene(obstacle_name)
this_object = self.obstacle_list[obstacle_name]
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose = copy.deepcopy(this_object.abs_pose)
robot_pose = self.get_robot_pose()
# p.pose.position.x -= robot_pose.position.x
# p.pose.position.y -= robot_pose.position.y
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = -euler[2]
quat = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
x = p.pose.position.x - robot_pose.position.x
y = p.pose.position.y - robot_pose.position.y
p.pose.position.x = math.cos(yaw) * x - math.sin(yaw) * y
p.pose.position.y = math.sin(yaw) * x + math.cos(yaw) * y
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(obstacle_name, p, size)
rospy.sleep(0.5)
def set_target_info(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
targets = objects_info["targets"]
target_name = targets.keys()
for name in target_name:
position = Point()
position.x = targets[name]["x"] - robot_x
position.y = targets[name]["y"] - robot_y
position.z = targets[name]["z"] - robot_z
self.goal_list[name] = position
def set_gripper_width_relationship(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
gripper_joint_value = objects_info["gripper_joint_value"]
objects_width = gripper_joint_value.keys()
for object_width in objects_width:
self.gripper_width[object_width] = gripper_joint_value[
object_width]
def get_object_list(self):
return self.object_list.keys()
def get_target_list(self):
return self.goal_list.keys()
def get_object_pose(self, object_name):
return copy.deepcopy(self.object_list[object_name].relative_pose)
def get_object_info(self, object_name):
this_object = copy.deepcopy(self.object_list[object_name])
pose = this_object.relative_pose
height = this_object.height
width = this_object.width
length = this_object.length
shape = this_object.shape
color = this_object.color
return pose, height, width, length, shape, color
def get_target_position(self, target_name):
position = copy.deepcopy(self.goal_list[target_name])
# print("before transformation",position)
p = Pose()
p.position = position
robot_pose = self.get_robot_pose()
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
yaw = -euler[2]
x = position.x - robot_pose.position.x
y = position.y - robot_pose.position.y
position.x = math.cos(yaw) * x - math.sin(yaw) * y
position.y = math.sin(yaw) * x + math.cos(yaw) * y
# print("after transformation",position, yaw)
position = self.baselink2arm(p).position
return position
def robot_pose_callback(self, msg):
self.robot_pose.position = msg.pose.pose.position
self.robot_pose.orientation = msg.pose.pose.orientation
def get_robot_pose(self):
# try:
# listener = tf.TransformListener()
# (trans,rot) = listener.lookupTransform('/map', "/base_link", rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# rospy.loginfo("no transformation")
# return
# pose = Pose()
# pose.position.x = trans[0]
# pose.position.y = trans[1]
# pose.position.z = trans[2]
# pose.orientation.x = rot[0]
# pose.orientation.y = rot[1]
# pose.orientation.z = rot[2]
# pose.orientation.w = rot[3]
return self.robot_pose
def get_arm_to_baselink(self):
# try:
# listener = tf.TransformListener()
# (trans,rot) = listener.lookupTransform('/base_link', "/ur10_base_link", rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# rospy.loginfo("no transformation")
# return
# self.transform_arm_to_baselink.x = trans[0]
# self.transform_arm_to_baselink.y = trans[1]
# self.transform_arm_to_baselink.z = trans[2]
# print(self.transform_arm_to_baselink)
self.transform_arm_to_baselink.x = 0.205
self.transform_arm_to_baselink.y = 0
self.transform_arm_to_baselink.z = 0.802
def get_workspace(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
workspace = joints_setup["workspace"]
x = workspace["center"]["x"]
y = workspace["center"]["y"]
z = workspace["center"]["z"]
min_r = workspace["r"]["min"]
max_r = workspace["r"]["max"]
min_z = workspace["min_z"]
self.workspace = WorkSpace(x, y, z, min_r, max_r, min_z)
# check if the position is inside workspace
def is_inside_workspace(self, x, y, z):
if z > self.workspace.min_z:
dx = x - self.workspace.x
dy = y - self.workspace.y
dz = z - self.workspace.z
r = math.sqrt(dx**2 + dy**2 + dz**2)
if self.workspace.min_r < r < self.workspace.max_r:
return True
return False
# move one joint of the arm to value
def set_arm_joint(self, joint_id, value):
joint_goal = self.arm.get_current_joint_values()
joint_goal[joint_id - 1] = value
self.arm.go(joint_goal, wait=True)
self.arm.stop()
# Forward Kinematics (FK): move the arm by axis values
def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4,
joint_5):
joint_goal = self.arm.get_current_joint_values()
joint_goal[0] = joint_0
joint_goal[1] = joint_1
joint_goal[2] = joint_2
joint_goal[3] = joint_3
joint_goal[4] = joint_4
joint_goal[5] = joint_5
self.arm.go(joint_goal, wait=True)
self.arm.stop() # To guarantee no residual movement
def get_joint_value(self, joint_id):
joints = self.arm.get_current_joint_values()
return joints[joint_id - 1]
def get_joints_value(self):
joints = self.arm.get_current_joint_values()
return joints
def get_arm_pose(self):
pose = self.arm.get_current_pose().pose
pose = self.baselink2arm(pose)
pose = self.TCP2gripper(pose, self.gripper_length)
# print(pose)
return self.msg2pose(pose)
def set_random_pose(self):
self.arm.set_random_target()
def set_gripper_length(self, length):
self.gripper_length = length
def set_home_value(self, home_value):
self.home_value = home_value
def back_to_home(self, move_gripper=True):
j1, j2, j3, j4, j5, j6, g = self.home_value
self.move_joint_arm(j1, j2, j3, j4, j5, j6)
if move_gripper:
self.move_joint_hand(g)
def set_pick_place_home_value(self, home_value):
self.pick_place_home_value = home_value
def move_to_pick_place_home(self, move_gripper=True):
j1, j2, j3, j4, j5, j6, g = self.pick_place_home_value
self.move_joint_arm(j1, j2, j3, j4, j5, j6)
if move_gripper:
self.move_joint_hand(g)
def fold_robot_arm(self, ):
self.move_joint_arm(0, 0, 0, 0, 0, 0)
self.move_joint_hand(0)
def gripper2TCP(self, pose, length=0):
roll, pitch, yaw, x, y, z = self.msg2pose(pose)
T = euler_matrix(roll, pitch, yaw)
T[0:3, 3] = numpy.array([x, y, z])
pos_gripper_tcp = numpy.array([0, -length, 0, 1])
pos_tcp = T.dot(pos_gripper_tcp)
pose.position.x = pos_tcp[0]
pose.position.y = pos_tcp[1]
pose.position.z = pos_tcp[2]
return pose
def TCP2gripper(self, pose, length=0):
roll, pitch, yaw, x, y, z = self.msg2pose(pose)
T = euler_matrix(roll, pitch, yaw)
T[0:3, 3] = numpy.array([x, y, z])
pos_gripper_tcp = numpy.array([0, length, 0, 1])
pos_tcp = T.dot(pos_gripper_tcp)
pose.position.x = pos_tcp[0]
pose.position.y = pos_tcp[1]
pose.position.z = pos_tcp[2]
return pose
def arm2baselink(self, in_pose):
pose = copy.deepcopy(in_pose)
pose.position.x += self.transform_arm_to_baselink.x
pose.position.y += self.transform_arm_to_baselink.y
pose.position.z += self.transform_arm_to_baselink.z
return pose
def baselink2arm(self, in_pose):
pose = copy.deepcopy(in_pose)
pose.position.x -= self.transform_arm_to_baselink.x
pose.position.y -= self.transform_arm_to_baselink.y
pose.position.z -= self.transform_arm_to_baselink.z
return pose
# Inverse Kinematics (IK): move TCP to given position and orientation
def move_pose_arm(self, pose_goal):
# pose_goal = self.pose2msg(roll, pitch, yaw, x, y, z)
# pose_goal = self.gripper2TCP(pose_goal, self.gripper_length)
x = pose_goal.position.x
y = pose_goal.position.y
z = pose_goal.position.z
if not self.is_inside_workspace(x, y, z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
return False
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
return True
# Move gripper
def move_joint_hand(self, finger1_goal, finger2_goal=10, finger3_goal=10):
if finger2_goal == 10 and finger3_goal == 10:
finger2_goal, finger3_goal = finger1_goal, finger1_goal
jointtrajectory = JointTrajectory()
jointtrajectory.header.stamp = rospy.Time.now()
jointtrajectory.joint_names.extend(["H1_F1J3", "H1_F2J3", "H1_F3J3"])
joint = JointTrajectoryPoint()
joint.positions.extend([finger1_goal, finger2_goal, finger3_goal])
joint.time_from_start = rospy.Duration(1)
jointtrajectory.points.append(joint)
self.gripperpub.publish(jointtrajectory)
def pose2msg(self, roll, pitch, yaw, x, y, z):
pose = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose.position.x = x
pose.position.y = y
pose.position.z = z
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def set_grasp_distance(self, min_distance, desired_distance):
self.approach_retreat_min_dist = min_distance
self.approach_retreat_desired_dist = desired_distance
def count_gripper_width(self, object_name):
object_width = self.object_list[object_name].width
if object_width in self.gripper_width:
return self.gripper_width[object_width]
else:
rospy.loginfo(
"Cannot find suitable gripper joint value for this object width"
)
return 0
# pick object to goal position
def pickup(self, object_name, position, width, distance=0.12):
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
rospy.loginfo('Stop placing')
return
pose = Pose()
pose.position = position
q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180))
pose.orientation = Quaternion(*q)
# transform from gripper to TCP
pose = self.gripper2TCP(pose, self.gripper_length)
pose.position.z += distance
rospy.loginfo('Start picking')
self.move_pose_arm(pose)
rospy.sleep(1)
# move down
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
print(wpose)
wpose.position.z -= distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
# pick
rospy.sleep(0.5)
self.move_joint_hand(width)
rospy.sleep(3)
# if object_name != "yellow_ball":
# self.arm.attach_object(object_name)
# move up
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
wpose.position.z += distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
rospy.loginfo('Pick finished')
# place object to goal position
def place(self, object_name, position, width=-0.2, distance=0.12):
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
rospy.loginfo('Stop placing')
return
pose = Pose()
pose.position = position
q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180))
pose.orientation = Quaternion(*q)
# transform from gripper to TCP
pose = self.gripper2TCP(pose, self.gripper_length)
pose.position.z += distance
rospy.loginfo('Start placing')
self.move_pose_arm(pose)
rospy.sleep(1)
# move down
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
print(wpose)
wpose.position.z -= distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
# place
self.move_joint_hand(width)
rospy.sleep(3)
# if object_name != "yellow_ball":
# self.arm.detach_object(object_name)
# self.clean_scene(object_name)
self.object_list.pop(object_name)
# move up
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
wpose.position.z += distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
rospy.loginfo('Place finished')
class Grasp_Place:
def __init__(self):
# Retrieve params:
#if there is no param named 'table_object_name' then use the default
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object1')
self._grasp_object_name_capscrew4 = rospy.get_param(
'~grasp_object_name4', 'capscrew4') #capscrew
self._grasp_object_name_screw = rospy.get_param(
'~grasp_object_name5', 'screw') # screw
#print self._grasp_object_name_capscrew4
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
# 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)
##############################################################################################################################
##############################################################################################################################
#Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
#get the pose of endfection
print(
self._arm.get_current_pose(self._arm.get_end_effector_link()).pose)
#self._Init_robot_state() #robot stata initilizations
#self._Go_to_default_pose() #set the robot pose to the 'home' pose set in moveit_setup_assistant (before a grasp)
#self._move_to_obj_for_grasp(Grasp_pose_use, gripper_depth) #move to the taget object for grasping
#self._grasp_action(GRIPPER_CLOSED) #Close the gripper to grasp the object
#self._move_up(displacementUp) #move up after the gripper grasp the target object
#self._move_to_placeGoal(place_Pose_use, gripper_depth) #move to the above area of the placeGoal pose(a little bit higher)
#self._move_down(displacementDown) #move down to the placeGoal pose
#self._place_action() #place -> release or open the gripper
#self._Go_to_default_pose() #set the robot pose to the 'home' pose set in moveit_setup_assistant (after a place)
#Costom_Command (Int)
# 0: _Init_robot_state()
# 1: _Go_to_default_pose()
# 2: _move_to_obj_for_grasp(Grasp_pose_use, gripper_depth)
# 3: _grasp_action(GRIPPER_CLOSED)
# 4: _move_up(displacementUp)
# 5: _move_to_placeGoal(place_Pose_use, gripper_depth)
# 6: _move_down(displacementDown)
# 7: _place_action()
##########Receive Command and respongding data from the server###########
# _Go_to_default_pose() #让机械臂到达默认位姿
# _move_to_obj_for_grasp(Grasp_pose_use, gripper_depth) # 移->到达螺母预抓取位姿态
# _grasp_action(GRIPPER_CLOSED) # 抓->控制爪子关闭
# _move_up(displacementUp) # 移->向上提螺母
# _move_to_placeGoal(place_Pose_use, gripper_depth) #移->趋向螺栓
# _move_down(displacementDown) # 移-> 插入螺栓
# _place_action() # 放->控制爪子放开
self._Init_robot_state()
#Initialzing the state of the robot
def _Init_robot_state(self):
rospy.loginfo('Initializing the robot state')
self._arm.set_goal_position_tolerance(0.001)
self._arm.set_goal_orientation_tolerance(0.001)
self._arm.allow_replanning(True)
self._arm.set_pose_reference_frame(self._robot.get_planning_frame())
self._arm.set_planning_time(7)
#Set the 'home' pose of the ur5 before graspsing or after placing sth
def _Go_to_default_pose(self):
rospy.set_param('Excute_Lock', False)
rospy.loginfo('Set the defaulit pose of the ur5 ')
self._arm.set_named_target(
'home'
) #go to the 'home' place that are set in moveit_setup_assistant.
self._arm.go()
self._gripper.set_joint_value_target(
'gripper_finger1_joint',
[0.0]) #[0.0] is the wide open pose of the gripper group.
self._gripper.go()
#move to the object for grasp
#gripper_depth : default->0.15
def _move_to_obj_for_grasp(self, pose, gripper_depth):
rospy.set_param('Excute_Lock', False)
#Step-1##move-to-the-object-for-grasp###
rospy.loginfo('moving-to-the-object-for-grasp')
pose.position.z = pose.position.z + gripper_depth
self._arm.set_pose_target(pose, self._arm.get_end_effector_link())
traj = self._arm.plan()
self._arm.execute(traj)
#rospy.sleep(1)
#grasp action
#gripper_depth : default->0.15
#GRIPPER_CLOSED is a list type and can be modified accodring to the object width
def _grasp_action(self, GRIPPER_CLOSED):
rospy.set_param('Excute_Lock', False)
G_C = [GRIPPER_CLOSED]
#Step-2##grasp###
#grasp_clsoe posture()
rospy.loginfo('grasping the object')
self._gripper.set_joint_value_target('gripper_finger1_joint', G_C)
self._gripper.go()
#rospy.sleep(1)
#move up 0.1
def _move_up(self, displacementUp):
rospy.set_param('Excute_Lock', False)
#Step-3##move-up after the grsap###
rospy.loginfo('moving up')
current_pose = self._arm.get_current_pose(
self._arm.get_end_effector_link(
)).pose #get the current pose of the end_effector
current_pose.position.z = current_pose.position.z + displacementUp
self._arm.set_pose_target(current_pose,
self._arm.get_end_effector_link())
traj = self._arm.plan()
self._arm.execute(traj)
#rospy.sleep(1)
#move to the placeGoal
def _move_to_placeGoal(self, pose, gripper_depth):
rospy.set_param('Excute_Lock', False)
#Step-4##move-to-the-placeGoal(a little bit higer in case of collision)###
rospy.loginfo(
'move-to-the-placeGoal: a little bit higer in case of collision')
pose.position.z = pose.position.z + gripper_depth
self._arm.set_pose_target(pose, self._arm.get_end_effector_link())
traj = self._arm.plan()
self._arm.execute(traj)
#rospy.sleep(1)
#maybe->Step-5##move-down-to the placeGoal###
def _move_down(self, displacementDown):
rospy.set_param('Excute_Lock', False)
#Step-5##move-down-to the placeGoal###
rospy.loginfo('moving down')
current_pose = self._arm.get_current_pose(
self._arm.get_end_effector_link(
)).pose #get the current pose of the end_effector
current_pose.position.z = current_pose.position.z - displacementDown
self._arm.set_pose_target(current_pose,
self._arm.get_end_effector_link())
traj = self._arm.plan()
self._arm.execute(traj)
#rospy.sleep(1)
#maybe->Step-5##move-down-to the placeGoal###
def _place_action(self):
rospy.set_param('Excute_Lock', False)
#Step-6##place###
#grasp_clsoe posture()
rospy.loginfo('opening the gripper and placing the object')
self._gripper.set_joint_value_target('gripper_finger1_joint', [0.0])
self._gripper.go()
class Pick_Place:
def __init__(self):
self.object_name = "box"
self.object_width = 0.03
self.arm_group = "irb_120"
self.gripper_group = "robotiq_85"
self.arm = moveit_commander.MoveGroupCommander("irb_120")
self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(1.0)
self.scene.remove_world_object(self.object_name)
self.pose_object = self.add_object(self.object_name)
rospy.sleep(1.0)
self.pose_object.position.z += 0.16
print(self.pose_object.position)
self.pose_place = Pose()
self.pose_place.position.x = self.pose_object.position.x
self.pose_place.position.y = self.pose_object.position.y - 0.1
self.pose_place.position.z = 0 #self.pose_object.position.z - 0.1
self.pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
self.approach_retreat_desired_dist = 0.2
self.approach_retreat_min_dist = 0.1
#self.grasp_action = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
# self.pickup_action = SimpleActionClient("/pickup", PickupAction)
# self.place_action = SimpleActionClient("/place", PlaceAction)
print("start pick and place")
# Pick
# while not self.pickup(self.arm_group, self.object_name, self.object_width):
# rospy.logwarn('Pick up failed! Retrying ...')
# rospy.sleep(1.0)
grasps = self.generate_grasps(self.pose_object, self.object_width)
self.arm.pick(self.object_name, grasps)
rospy.loginfo('Pick up successfully')
rospy.sleep(1)
# self.arm.detach_object(self.object_name)
# self.clean_scene()
# print("pose_place: ", self.pose_place)
# Place
# while not self.place(self.arm_group, self.object_name, self.pose_place):
# rospy.logwarn('Place failed! Retrying ...')
# rospy.sleep(1.0)
place = self.generate_places(self.pose_place)
self.arm.place(self.object_name, place)
rospy.loginfo('Place successfully')
self.move_joint_hand(0)
rospy.loginfo("Moving arm to HOME point")
self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6)
rospy.sleep(1)
self.arm.detach_object(self.object_name)
self.clean_scene()
def clean_scene(self):
self.scene.remove_world_object(self.object_name)
def add_object(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.015
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
size = (0.03, 0.03, 0.03)
self.scene.add_box(name, p, size)
return p.pose
def move_pose_arm(self, roll, pitch, yaw, x, y, z):
pose_goal = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose_goal.orientation.x = quat[0]
pose_goal.orientation.y = quat[1]
pose_goal.orientation.z = quat[2]
pose_goal.orientation.w = quat[3]
pose_goal.position.x = x
pose_goal.position.y = y
pose_goal.position.z = z
self.arm.set_pose_target(pose_goal)
plan = self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
# Move the Robotiq gripper by master axis
def move_joint_hand(self, gripper_finger1_joint):
joint_goal = self.gripper.get_current_joint_values()
joint_goal[2] = gripper_finger1_joint # Gripper master axis
self.gripper.go(joint_goal, wait=True)
self.gripper.stop() # To guarantee no residual movement
def generate_grasps(self, pose, width):
grasps = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
numpy.deg2rad(90.0)):
# Create place location:
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.2
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.2
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
grasp.max_contact_force = 100
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.55)
#traj.effort.append(100)
traj.time_from_start = rospy.Duration.from_sec(1.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
def generate_places(self, target):
#places = []
now = rospy.Time.now()
# for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(30.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.0)
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 = -0.2
# 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 = 0.2
place.allowed_touch_objects.append(self.object_name)
place.post_place_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0)
#traj.effort.append(0)
traj.time_from_start = rospy.Duration.from_sec(1.0)
place.post_place_posture.points.append(traj)
# Add place:
#places.append(place)
return place
def create_pickup_goal(self, group, target, grasps):
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.allowed_planning_time = 3.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):
goal = PlaceGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(places)
goal.allowed_planning_time = 3.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):
print("start grnerating grasps")
grasps = self.generate_grasps(self.pose_object, width)
print("start creating pickup goal")
goal = self.create_pickup_goal(group, target, grasps)
state = self.pickup_action.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' %
self.pickup_action.get_goal_status_text())
return None
result = self.pickup_action.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):
places = self.generate_places(self.pose_object, width)
goal = self.create_place_goal(group, target, places)
state = self.place_action.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' %
self.place_action.get_goal_status_text())
return None
result = self.place_action.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
class Pick_Place:
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 _add_table(self, name):
rospy.loginfo("entered table")
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.2
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, (9, 9, 0.02))
return p.pose
def _add_grasp_block_(self, name):
rospy.loginfo("entered box grabber")
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.05
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.1, 0.1, 0.1))
return p.pose
class Pick_Place:
def __init__(self):
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_kinematics', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
jointslimit = joints_setup["joints_limit"]
home_value = joints_setup["home_value"]
j1 = home_value["joint_1"]
j2 = home_value["joint_2"]
j3 = home_value["joint_3"]
j4 = home_value["joint_4"]
j5 = home_value["joint_5"]
j6 = home_value["joint_6"]
g = home_value["gripper"]
self.set_home_value([j1, j2, j3, j4, j5, j6, g])
self.object_list = {}
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_kinematics', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
robot_roll = objects_info["robot"]["pose"]["roll"]
robot_pitch = objects_info["robot"]["pose"]["pitch"]
robot_yaw = objects_info["robot"]["pose"]["yaw"]
rospy.loginfo("Spawning Objects in Gazebo and planning scene")
objects = objects_info["objects"]
objects_name = objects.keys()
for object_name in objects_name:
name = object_name
shape = objects[name]["shape"]
color = objects[name]["color"]
x = objects[name]["pose"]["x"]
y = objects[name]["pose"]["y"]
z = objects[name]["pose"]["z"]
roll = objects[name]["pose"]["roll"]
pitch = objects[name]["pose"]["pitch"]
yaw = objects[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = x - robot_x
p.pose.position.y = y - robot_y
p.pose.position.z = z - robot_z
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation = Quaternion(*q)
if shape == "box":
x = objects[name]["size"]["x"]
y = objects[name]["size"]["y"]
z = objects[name]["size"]["z"]
p.pose.position.z += z / 2
height = z
width = y
length = x
self.object_list[name] = Object(p.pose, object_pose,
height, width, length,
shape, color)
elif shape == "cylinder":
height = objects[name]["size"]["height"]
radius = objects[name]["size"]["radius"]
p.pose.position.z += height / 2
self.object_list[name] = Object(p.pose, object_pose,
height, radius * 2,
radius * 2, shape, color)
elif shape == "sphere":
radius = objects[name]["size"]
p.pose.position.z += radius
self.object_list[name] = Object(p.pose, object_pose,
radius * 2, radius * 2,
radius * 2, shape, color)
# self.object_list = object_list
self.goal_list = {}
self.set_target_info()
self.gripper_width = {}
self.set_gripper_width_relationship()
self.arm = moveit_commander.MoveGroupCommander("irb_120")
self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")
self.arm.set_goal_tolerance(0.01)
# set default grasp message infos
self.set_grasp_distance(0.1, 0.2)
self.set_grasp_direction(0, 0, -0.5)
self.get_workspace()
self.message_pub = rospy.Publisher("/gui_message",
String,
queue_size=0)
self.updatepose_pub = rospy.Publisher("/updatepose",
Bool,
queue_size=0)
def send_message(self, message):
msg = String()
msg.data = message
self.message_pub.publish(msg)
def updatepose_trigger(self, value):
msg = Bool()
msg.data = value
self.updatepose_pub.publish(msg)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def set_target_info(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_kinematics', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
targets = objects_info["targets"]
target_name = targets.keys()
for name in target_name:
position = Point()
position.x = targets[name]["x"] - robot_x
position.y = targets[name]["y"] - robot_y
position.z = targets[name]["z"] - robot_z
self.goal_list[name] = position
def set_gripper_width_relationship(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_kinematics', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
gripper_joint_value = objects_info["gripper_joint_value"]
objects_width = gripper_joint_value.keys()
for object_width in objects_width:
self.gripper_width[object_width] = gripper_joint_value[
object_width]
def get_object_list(self):
return self.object_list.keys()
def get_target_list(self):
return self.goal_list.keys()
def get_object_pose(self, object_name):
return copy.deepcopy(self.object_list[object_name].relative_pose)
def get_object_info(self, object_name):
this_object = copy.deepcopy(self.object_list[object_name])
pose = this_object.relative_pose
height = this_object.height
width = this_object.width
length = this_object.length
shape = this_object.shape
color = this_object.color
return pose, height, width, length, shape, color
def get_target_position(self, target_name):
return self.goal_list[target_name]
def pose2msg(self, roll, pitch, yaw, x, y, z):
pose = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose.position.x = x
pose.position.y = y
pose.position.z = z
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def pose2msg_deg(self, roll, pitch, yaw, x, y, z):
pose = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(numpy.deg2rad(roll), numpy.deg2rad(pitch),
numpy.deg2rad(yaw))
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose.position.x = x
pose.position.y = y
pose.position.z = z
return pose
def msg2pose_deg(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = numpy.rad2deg(euler[0])
pitch = numpy.rad2deg(euler[1])
yaw = numpy.rad2deg(euler[2])
return roll, pitch, yaw, x, y, z
def get_workspace(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_kinematics', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
workspace = joints_setup["workspace"]
x = workspace["center"]["x"]
y = workspace["center"]["y"]
z = workspace["center"]["z"]
min_r = workspace["r"]["min"]
max_r = workspace["r"]["max"]
min_z = workspace["min_z"]
self.workspace = WorkSpace(x, y, z, min_r, max_r, min_z)
# check if the position is inside workspace
def is_inside_workspace(self, x, y, z):
if z > self.workspace.min_z:
dx = x - self.workspace.x
dy = y - self.workspace.y
dz = z - self.workspace.z
r = math.sqrt(dx**2 + dy**2 + dz**2)
if self.workspace.min_r < r < self.workspace.max_r:
return True
return False
def gripper2TCP(self, pose, length=0):
roll, pitch, yaw, x, y, z = self.msg2pose(pose)
T = euler_matrix(roll, pitch, yaw)
T[0:3, 3] = numpy.array([x, y, z])
pos_gripper_tcp = numpy.array([-length, 0, 0, 1])
pos_tcp = T.dot(pos_gripper_tcp)
pose.position.x = pos_tcp[0]
pose.position.y = pos_tcp[1]
pose.position.z = pos_tcp[2]
return pose
def TCP2gripper(self, pose, length=0):
roll, pitch, yaw, x, y, z = self.msg2pose(pose)
T = euler_matrix(roll, pitch, yaw)
T[0:3, 3] = numpy.array([x, y, z])
pos_gripper_tcp = numpy.array([length, 0, 0, 1])
pos_tcp = T.dot(pos_gripper_tcp)
pose.position.x = pos_tcp[0]
pose.position.y = pos_tcp[1]
pose.position.z = pos_tcp[2]
return pose
def set_home_value(self, home_value):
self.home_value = home_value
def back_to_home(self):
j1, j2, j3, j4, j5, j6, g = self.home_value
self.move_joint_arm(j1, j2, j3, j4, j5, j6)
self.move_joint_hand(g)
rospy.sleep(1)
# Forward Kinematics (FK): move the arm by axis values
def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4,
joint_5):
joint_goal = self.arm.get_current_joint_values()
joint_goal[0] = joint_0
joint_goal[1] = joint_1
joint_goal[2] = joint_2
joint_goal[3] = joint_3
joint_goal[4] = joint_4
joint_goal[5] = joint_5
self.arm.go(joint_goal, wait=True)
self.arm.stop() # To guarantee no residual movement
self.updatepose_trigger(True)
# Inverse Kinematics: Move the robot arm to desired pose
def move_pose_arm(self, pose_goal):
position = pose_goal.position
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
return
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
self.updatepose_trigger(True)
# Move the Robotiq gripper by master axis
def move_joint_hand(self, gripper_finger1_joint):
joint_goal = self.gripper.get_current_joint_values()
joint_goal[2] = gripper_finger1_joint # Gripper master axis
self.gripper.go(joint_goal, wait=True)
self.gripper.stop() # To guarantee no residual movement
self.updatepose_trigger(True)
def set_grasp_direction(self, x, y, z):
self.approach_direction = Vector3()
self.approach_direction.x = x
self.approach_direction.y = y
self.approach_direction.z = z
self.retreat_direction = Vector3()
self.retreat_direction.x = -x
self.retreat_direction.y = -y
self.retreat_direction.z = -z
def set_grasp_distance(self, min_distance, desired_distance):
self.approach_retreat_min_dist = min_distance
self.approach_retreat_desired_dist = desired_distance
def count_gripper_width(self, object_name):
object_width = self.object_list[object_name].width
if object_width in self.gripper_width:
return self.gripper_width[object_width]
else:
rospy.loginfo(
"Cannot find suitable gripper joint value for this object width"
)
return 0
def generate_grasp(self,
object_name,
eef_orientation,
position,
width=0,
roll=0,
pitch=0,
yaw=0,
length=0):
if width == 0: # need to count gripper joint value
if (eef_orientation == "horizontal"
and pitch == 0) or (eef_orientation == "vertical"
and yaw == 0):
width = self.count_gripper_width(object_name)
else:
rospy.loginfo(
"Orientation doesn't meet requirement. Please tune gripper width by yourself"
)
return self.generate_grasp_width(eef_orientation, position, width,
roll, pitch, yaw, length)
def generate_grasp_width(self,
eef_orientation,
position,
width,
roll=0,
pitch=0,
yaw=0,
length=0):
now = rospy.Time.now()
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose.position = position
if eef_orientation == "horizontal":
q = quaternion_from_euler(0.0, numpy.deg2rad(pitch), 0.0)
elif eef_orientation == "vertical":
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0),
numpy.deg2rad(yaw))
elif eef_orientation == "user_defined":
q = quaternion_from_euler(numpy.deg2rad(roll),
numpy.deg2rad(pitch), numpy.deg2rad(yaw))
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# transform from gripper to TCP
grasp.grasp_pose.pose = self.gripper2TCP(grasp.grasp_pose.pose, length)
if not self.is_inside_workspace(grasp.grasp_pose.pose.position.x,
grasp.grasp_pose.pose.position.y,
grasp.grasp_pose.pose.position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
#return False
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector = self.approach_direction
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector = self.retreat_direction
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
grasp.max_contact_force = 1000
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.0)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(width)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
return grasp
# pick up object with grasps
def pickup(self, object_name, grasps):
rospy.loginfo('Start picking ' + object_name)
self.arm.pick(object_name, grasps)
#self.gripper.stop()
self.updatepose_trigger(True)
rospy.loginfo('Pick up finished')
self.arm.detach_object(object_name)
self.clean_scene(object_name)
# place object to goal position
def place(self,
eef_orientation,
position,
distance=0.1,
roll=0,
pitch=0,
yaw=180):
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
rospy.loginfo('Stop placing')
return
pose = Pose()
pose.position = position
if eef_orientation == "horizontal":
q = quaternion_from_euler(0.0, numpy.deg2rad(pitch),
numpy.deg2rad(180))
elif eef_orientation == "vertical":
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0),
numpy.deg2rad(yaw))
elif eef_orientation == "user_defined":
q = quaternion_from_euler(numpy.deg2rad(roll),
numpy.deg2rad(pitch), numpy.deg2rad(yaw))
pose.orientation = Quaternion(*q)
pose.position.z += distance
rospy.loginfo('Start placing')
self.move_pose_arm(pose)
rospy.sleep(1)
# move down
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z -= distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
# place
self.move_joint_hand(0)
rospy.sleep(1)
# move up
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z += distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
rospy.loginfo('Place finished')
class Pick_Place:
def __init__(self):
# 检索参数:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._table2_object_name = rospy.get_param('~table_object_name', 'Grasp_Table2')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object2')
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)
# 创建（调试）发布者:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# 创建规划现场，机器人指挥官:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# 清理现场:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._table2_object_name)
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._grasp_object2_name)
# 将表和可乐罐对象添加到计划场景:
self._pose_table = self._add_table(self._table_object_name)
self._pose_table = self._add_table2(self._table2_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
self._pose_coke_can2 = self._add_grasp_block2_(self._grasp_object2_name)
rospy.sleep(1.0)
# 定义目标位置姿势:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x-0.5
self._pose_place.position.y = self._pose_coke_can.position.y-0.85
self._pose_place.position.z = self._pose_coke_can.position.z-0.3
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above
self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above
# 检索组 (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
self._arm.set_named_target('pickup')
self._arm.go(wait=True)
print("Pickup pose")
# 创建抓取生成器“生成”动作客户端:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(1.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# 创建移动组“zhua取”操作客户端:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(1.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# 创建移动组“放置”动作客户端:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(1.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')
print("pose_place: ", self._pose_place)
# 放置可乐可能会在支撑表面（桌子）上的其他地方产生异物:
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')
print "-------------test1--------------"
rospy.sleep(1.0)
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._table2_object_name)
print "-------------test2------------------"
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.85
p.pose.position.y = 0.0
p.pose.position.z = 0.70
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, (1, 1, 0.05))
print "-------------test3------------------"
return p.pose
def _add_table2(self, name):
"""
创建表并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.85
p.pose.position.z = 0.40
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.5, 0.05))
print "-------------test4------------------"
return p.pose
def _add_grasp_block_(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.74
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.045, 0.045, 0.045))
print "-------------test5------------------"
return p.pose
def _add_grasp_block2_(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.3
p.pose.position.z = 0.74
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.045, 0.045, 0.045))
print "-------------test6------------------"
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.
使用抓握生成器生成动作来生成抓握； 基于moveit_simple_grasps pkg上的server_test.py示例
Generate the grasp Pose Array data for visualization,
and then send the grasp goal to the grasp server.
生成抓取姿势阵列数据以进行可视化，然后将抓取目标发送到抓取服务器。
"""
# 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)
# 发送目标并等待结果:
# 将目标发送到ActionServer，等待目标完成，并且必须抢占目标.
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
# 发布掌握（用于调试/可视化目的）:
self._publish_grasps(grasps)
print "-------------test7------------------"
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)
# 生成预安置方法:
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 = 0.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 = 0.1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
print "-------------test8------------------"
return places
def _create_pickup_goal(self, group, target, grasps):
"""
创建一个MoveIt！ 接送目标
创建一个捡起抓取物体的目标
"""
# 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
# 配置目标计划选项:
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
print "-------------test9------------------"
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
Create a place goal for MoveIt!
"""
# 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
print "-------------test10------------------"
return goal
def _pickup(self, group, target, width):
"""
使用计划小组选择目标
"""
# 从掌握生成器服务器获取可能的掌握:
grasps = self._generate_grasps(self._pose_coke_can, width)
# 创建并发送提货目标:
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
print "-------------test11------------------"
return True
def _place(self, group, target, place):
"""
使用计划组放置目标
"""
# 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
print "-------------test12------------------"
return True
def _publish_grasps(self, grasps):
"""
使用PoseArray消息将抓取发布为姿势
"""
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)
print "-------------test13------------------"
def _publish_places(self, places):
"""
使用PoseArray消息将位置发布为姿势
"""
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)
print "-------------test14------------------"
def main(argv):
#moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("PickPlaceDemo")
rospy.on_shutdown(onshutdownhook)
transform = tf.TransformListener()
global scene
scene = PlanningSceneInterface()
robot = RobotCommander()
group = MoveGroupCommander("manipulator")
rospy.sleep(2)
#scene.is_diff = True
#t = transform.getLatestCommonTime("/tool0", "/world")
#tool0_pose, tool0_quaternion = transform.lookupTransform("world", "tool0", t)
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.700
p.pose.position.y = 0.000
p.pose.position.z = 0.060
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("box1", p, (0.15, 0.15, 0.15))
p.pose.position.x = -0.700
p.pose.position.y = -0.250
p.pose.position.z = 0.060
scene.add_box("box2", p, (0.15, 0.15, 0.15))
p.pose.position.x = -0.700
p.pose.position.y = -0.500
p.pose.position.z = 0.060
scene.add_box("box3", p, (0.15, 0.15, 0.15))
rospy.sleep(5)
quaternion = tf.transformations.quaternion_from_euler(
0.0, (math.pi / 2.0), 0.0)
pose_target = Pose()
pose_target.position.x = -0.700
pose_target.position.y = 0.000
pose_target.position.z = 0.155
pose_target.orientation.x = quaternion[0]
pose_target.orientation.y = quaternion[1]
pose_target.orientation.z = quaternion[2]
pose_target.orientation.w = quaternion[3]
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.attach_box("tool0", "box1")
rospy.sleep(1)
pose_target.position.x = 0.000
pose_target.position.y = -0.700
pose_target.position.z = 0.155
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.remove_attached_object("tool0")
rospy.sleep(1)
pose_target.position.x = -0.700
pose_target.position.y = -0.250
pose_target.position.z = 0.155
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.attach_box("tool0", "box2")
rospy.sleep(1)
pose_target.position.x = 0.000
pose_target.position.y = -0.700
pose_target.position.z = 0.306
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.remove_attached_object("tool0")
rospy.sleep(1)
pose_target.position.x = -0.700
pose_target.position.y = -0.500
pose_target.position.z = 0.155
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.attach_box("tool0", "box3")
rospy.sleep(1)
pose_target.position.x = 0.000
pose_target.position.y = -0.700
pose_target.position.z = 0.457
group.set_pose_target(pose_target)
group.go(wait=True)
rospy.sleep(1)
scene.remove_attached_object("tool0")
while not rospy.is_shutdown():
pass
class ModelManager:
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 pose2msg(self, x, y, z, roll, pitch, yaw):
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def spawn_model(self, model_name, model_pose):
with open(
os.path.join(rospkg.RosPack().get_path('ur5_gripper_demo'),
'models', model_name, 'model.sdf'), "r") as f:
model_xml = f.read()
self.spawn_model_srv(model_name, model_xml, "", model_pose, "world")
def delete_model(self, model_name):
self.delete_model_srv(model_name)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def respawn_all_objects(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
this_object = self.object_list[object_name]
print("Respawning {}".format(object_name))
# remove old objects in Gazebo
self.delete_model(object_name)
# respawn new objects in Gazebo
roll, pitch, yaw, x, y, z = self.msg2pose(this_object.abs_pose)
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
self.spawn_model(object_name, object_pose)
# respawn objects in Rviz
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(object_name)
p.pose = copy.copy(this_object.relative_pose)
shape = this_object.shape
if shape == "box":
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(object_name, p, size)
elif shape == "cylinder":
height = this_object.height
radius = this_object.width / 2
self.scene.add_cylinder(object_name, p, height, radius)
elif shape == "sphere":
radius = this_object.width / 2
self.scene.add_sphere(object_name, p, radius)
rospy.sleep(0.5)
def spawn_all_model(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_vacuum_gripper', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
robot_roll = objects_info["robot"]["pose"]["roll"]
robot_pitch = objects_info["robot"]["pose"]["pitch"]
robot_yaw = objects_info["robot"]["pose"]["yaw"]
rospy.loginfo("Spawning Objects in Gazebo")
objects = objects_info["objects"]
objects_name = objects.keys()
for object_name in objects_name:
name = object_name
shape = objects[name]["shape"]
color = objects[name]["color"]
# add object in Gazebo
# self.delete_model(object_name)
x = objects[name]["pose"]["x"]
y = objects[name]["pose"]["y"]
z = objects[name]["pose"]["z"]
roll = objects[name]["pose"]["roll"]
pitch = objects[name]["pose"]["pitch"]
yaw = objects[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
self.spawn_model(object_name, object_pose)
# add object in planning scene(Rviz)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(name)
p.pose.position.x = x - robot_x
p.pose.position.y = y - robot_y
p.pose.position.z = z - robot_z
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation = Quaternion(*q)
if shape == "box":
x = objects[name]["size"]["x"]
y = objects[name]["size"]["y"]
z = objects[name]["size"]["z"]
p.pose.position.z += z / 2
# size = (x, y, z)
# self.scene.add_box(name, p, size)
height = z
width = y
length = x
self.object_list[name] = Object(p.pose, object_pose,
height, width, length,
shape, color)
elif shape == "cylinder":
height = objects[name]["size"]["height"]
radius = objects[name]["size"]["radius"]
p.pose.position.z += height / 2
self.scene.add_cylinder(name, p, height, radius)
self.object_list[name] = Object(p.pose, object_pose,
height, radius * 2,
radius * 2, shape, color)
elif shape == "sphere":
radius = objects[name]["size"]
p.pose.position.z += radius
self.scene.add_sphere(name, p, radius)
self.object_list[name] = Object(p.pose, object_pose,
radius * 2, radius * 2,
radius * 2, shape, color)
rospy.sleep(0.5)
rospy.loginfo("Spawning Obstacles in planning scene")
obstacles = objects_info["obstacles"]
obstacles_name = obstacles.keys()
for obstacle_name in obstacles_name:
name = obstacle_name
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(name)
p.pose.position.x = obstacles[name]["pose"]["x"] - robot_x
p.pose.position.y = obstacles[name]["pose"]["y"] - robot_y
p.pose.position.z = obstacles[name]["pose"]["z"] - robot_z
q = quaternion_from_euler(robot_roll, robot_pitch, robot_yaw)
p.pose.orientation = Quaternion(*q)
x = obstacles[name]["size"]["x"]
y = obstacles[name]["size"]["y"]
z = obstacles[name]["size"]["z"]
size = (x, y, z)
self.scene.add_box(name, p, size)
rospy.sleep(0.5)
def main(argv):
#This should become dynamic, e.g. rosparam
offset = [-0.450, -0.450, 0.0]
UDP_IP = "127.0.0.1"
UDP_PORT = 9000
rospy.init_node("Collision_adder")
transform = tf.TransformListener()
rospack = rospkg.RosPack()
#while not t.canTransform("shoulder_link", "base_link", rospy.Time.now()):
# print "."
mesh_path = rospack.get_path("ur_description") + "/meshes/ur5/collision/"
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(2)
sock = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock.bind((UDP_IP, UDP_PORT))
#I assume that the base can not move, e.g. is mounted in a fixed location.
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 = offset[0]
p.pose.position.y = offset[1]
p.pose.position.z = offset[2]
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_mesh("base", p, mesh_path + "/base.stl")
#scene.is_diff = True
while not rospy.is_shutdown():
data, addr = sock.recvfrom(2024) # buffer size is 1024 bytes
print "received message:", data
jointstates = data.split(",")
#t = transform.getLatestCommonTime("/shoulder_link", "/base_link")
#shoulder_pose, shoulder_quaternion = transform.lookupTransform("base_link", "shoulder_link", t)
shoulderstate = jointstates[0].split(";")
shoulder = PoseStamped()
shoulder.header.frame_id = robot.get_planning_frame()
shoulder.pose.position.x = offset[0] + float(shoulderstate[1])
shoulder.pose.position.y = offset[1] + float(shoulderstate[2])
shoulder.pose.position.z = offset[2] + float(shoulderstate[3])
shoulder.pose.orientation.x = float(shoulderstate[4])
shoulder.pose.orientation.y = float(shoulderstate[5])
shoulder.pose.orientation.z = float(shoulderstate[6])
shoulder.pose.orientation.w = float(shoulderstate[7])
scene.add_mesh(shoulderstate[0], shoulder,
mesh_path + shoulderstate[0] + ".stl")
#t = transform.getLatestCommonTime("/upper_arm_link", "/base_link")
#upper_arm_pose, upper_arm_quaternion = transform.lookupTransform("base_link", "upper_arm_link", t)
upperarmstate = jointstates[1].split(";")
upper_arm = PoseStamped()
upper_arm.header.frame_id = robot.get_planning_frame()
upper_arm.pose.position.x = offset[0] + float(upperarmstate[1])
upper_arm.pose.position.y = offset[1] + float(upperarmstate[2])
upper_arm.pose.position.z = offset[2] + float(upperarmstate[3])
upper_arm.pose.orientation.x = float(upperarmstate[4])
upper_arm.pose.orientation.y = float(upperarmstate[5])
upper_arm.pose.orientation.z = float(upperarmstate[6])
upper_arm.pose.orientation.w = float(upperarmstate[7])
scene.add_mesh(upperarmstate[0], upper_arm,
mesh_path + upperarmstate[0] + ".stl")
#t = transform.getLatestCommonTime("/forearm_link", "/base_link")
#fore_arm_pose, fore_arm_quaternion = transform.lookupTransform("base_link", "forearm_link", t)
forearmstate = jointstates[2].split(";")
fore_arm = PoseStamped()
fore_arm.header.frame_id = robot.get_planning_frame()
fore_arm.pose.position.x = offset[0] + float(forearmstate[1])
fore_arm.pose.position.y = offset[1] + float(forearmstate[2])
fore_arm.pose.position.z = offset[2] + float(forearmstate[3])
fore_arm.pose.orientation.x = float(forearmstate[4])
fore_arm.pose.orientation.y = float(forearmstate[5])
fore_arm.pose.orientation.z = float(forearmstate[6])
fore_arm.pose.orientation.w = float(forearmstate[7])
scene.add_mesh(forearmstate[0], fore_arm,
mesh_path + forearmstate[0] + ".stl")
#t = transform.getLatestCommonTime("/wrist_1_link", "/base_link")
#wrist1_pose, wrist1_quaternion = transform.lookupTransform("base_link", "wrist_1_link", t)
wrist1state = jointstates[3].split(";")
wrist1 = PoseStamped()
wrist1.header.frame_id = robot.get_planning_frame()
wrist1.pose.position.x = offset[0] + float(wrist1state[1])
wrist1.pose.position.y = offset[1] + float(wrist1state[2])
wrist1.pose.position.z = offset[2] + float(wrist1state[3])
wrist1.pose.orientation.x = float(wrist1state[4])
wrist1.pose.orientation.y = float(wrist1state[5])
wrist1.pose.orientation.z = float(wrist1state[6])
wrist1.pose.orientation.w = float(wrist1state[7])
scene.add_mesh(wrist1state[0], wrist1,
mesh_path + wrist1state[0] + ".stl")
#t = transform.getLatestCommonTime("/wrist_2_link", "/base_link")
#wrist2_pose, wrist2_quaternion = transform.lookupTransform("base_link", "wrist_2_link", t)
wrist2state = jointstates[4].split(";")
wrist2 = PoseStamped()
wrist2.header.frame_id = robot.get_planning_frame()
wrist2.pose.position.x = offset[0] + float(wrist2state[1])
wrist2.pose.position.y = offset[1] + float(wrist2state[2])
wrist2.pose.position.z = offset[2] + float(wrist2state[3])
wrist2.pose.orientation.x = float(wrist2state[4])
wrist2.pose.orientation.y = float(wrist2state[5])
wrist2.pose.orientation.z = float(wrist2state[6])
wrist2.pose.orientation.w = float(wrist2state[7])
scene.add_mesh(wrist2state[0], wrist2,
mesh_path + wrist2state[0] + ".stl")
#t = transform.getLatestCommonTime("/wrist_3_link", "/base_link")
#wrist3_pose, wrist3_quaternion = transform.lookupTransform("base_link", "wrist_3_link", t)
wrist3state = jointstates[5].split(";")
wrist3 = PoseStamped()
wrist3.header.frame_id = robot.get_planning_frame()
wrist3.pose.position.x = offset[0] + float(wrist3state[1])
wrist3.pose.position.y = offset[1] + float(wrist3state[2])
wrist3.pose.position.z = offset[2] + float(wrist3state[3])
wrist3.pose.orientation.x = float(wrist3state[4])
wrist3.pose.orientation.y = float(wrist3state[5])
wrist3.pose.orientation.z = float(wrist3state[6])
wrist3.pose.orientation.w = float(wrist3state[7])
scene.add_mesh(wrist3state[0], wrist3,
mesh_path + wrist3state[0] + ".stl")
class DaArmServer:
"""The basic, design problem/tui agnostic arm server
"""
gestures = {}
grasp_height = 0.1
drop_height = 0.2
moving = False
paused = False
move_group_state = "IDLE"
def __init__(self, num_planning_attempts=20):
rospy.init_node("daarm_server", anonymous=True)
self.init_params()
self.init_scene()
self.init_publishers()
self.init_subscribers()
self.init_action_clients()
self.init_service_clients()
self.init_arm(num_planning_attempts)
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 init_scene(self):
world_objects = ["table", "tui", "monitor", "overhead", "wall"]
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.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.3556, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8636)
self.wallPose.pose.position = Point(-0.508, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
def init_params(self):
try:
self.grasp_height = get_ros_param("GRASP_HEIGHT", "Grasp height defaulting to 0.1")
self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.2")
except ValueError as e:
rospy.loginfo(e)
def init_publishers(self):
self.calibration_publisher = rospy.Publisher("/calibration_results", CalibrationParams)
self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs", String, queue_size=1)
rospy.sleep(0.5)
def init_subscribers(self):
self.joint_angle_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/joint_angles', JointAngles, self.update_joints)
self.move_it_feedback_subscriber = rospy.Subscriber(
'/move_group/feedback', MoveGroupActionFeedback, self.update_move_group_state)
def init_action_servers(self):
self.calibration_server = actionlib.SimpleActionServer("calibrate_arm", CalibrateAction, self.calibrate)
self.move_block_server = actionlib.SimpleActionServer("move_block", MoveBlockAction, self.handle_move_block)
#self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
def init_services(self):
self.home_arm_service = rospy.Service("/home_arm", ArmCommand, self.handle_home_arm)
# emergency stop
self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand, self.handle_stop_arm)
# stop and pause for a bit
self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand, self.handle_pause_arm)
self.start_arm_service = rospy.Service("/restart_arm", ArmCommand, self.handle_restart_arm)
def init_action_clients(self):
# Action Client for joint control
joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
self.joint_action_client = actionlib.SimpleActionClient(
joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
rospy.loginfo('Waiting for ArmJointAnglesAction server...')
self.joint_action_client.wait_for_server()
rospy.loginfo('ArmJointAnglesAction Server Connected')
# Service to move the gripper fingers
finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
self.finger_action_client = actionlib.SimpleActionClient(
finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
self.finger_action_client.wait_for_server()
def init_service_clients(self):
self.is_simulation = None
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def handle_start_arm(self, message):
return self.restart_arm()
def handle_stop_arm(self, message):
return self.stop_motion()
def handle_pause_arm(self, message):
self.stop_motion(home=True, pause=True)
return str(self.paused)
def handle_restart_arm(self, message):
self.restart_arm()
self.paused = False
return str(self.paused)
def handle_home_arm(self, message):
try:
status = self.home_arm_kinova()
return json.dumps(status)
except rospy.ServiceException as e:
rospy.loginfo("Homing arm failed")
def home_arm(self):
# send the arm home
# for now, let's just use the kinova home
self.home_arm_client()
def home_arm_kinova(self):
"""Takes the arm to the kinova default home if possible
"""
self.arm.set_named_target("Home")
try:
self.arm.go()
return "successful home"
except:
return "failed to home"
def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
finger_max_turn = 6800
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float((finger1_pct/100.0)*finger_max_turn)
goal.fingers.finger2 = float((finger2_pct/100.0)*finger_max_turn)
goal.fingers.finger3 = float((finger3_pct/100.0)*finger_max_turn)
self.finger_action_client.send_goal(goal)
if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
return self.finger_action_client.get_result()
else:
self.finger_action_client.cancel_all_goals()
rospy.loginfo('the gripper action timed-out')
return None
def move_joint_angles(self, angle_set):
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angle_set[0]
goal.angles.joint2 = angle_set[1]
goal.angles.joint3 = angle_set[2]
goal.angles.joint4 = angle_set[3]
goal.angles.joint5 = angle_set[4]
goal.angles.joint6 = angle_set[5]
goal.angles.joint7 = angle_set[6]
self.joint_action_client.send_goal(goal)
if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
return self.joint_action_client.get_result()
else:
print(' the joint angle action timed-out')
self.joint_action_client.cancel_all_goals()
return None
def handle_move_block(self, message):
"""msg format: {id: int,
source: Point {x: float,y: float},
target: Point {x: float, y: float}
"""
def open_gripper(self, delay=0):
"""open the gripper ALL THE WAY, then delay
"""
if self.is_simulation is True:
self.gripper.set_named_target("Open")
self.gripper.go()
else:
self.move_fingers(50, 50, 50)
rospy.sleep(delay)
def close_gripper(self, delay=0):
"""close the gripper ALL THE WAY, then delay
"""
self.gripper.set_named_target("Close")
self.gripper.go()
rospy.sleep(delay)
def handle_gesture(self, gesture):
# lookup the gesture from a table? or how about a message?
# one possibility, object that contains desired deltas in pos/orientation
# as well as specify the arm or gripper choice
pass
def move_arm_to_pose(self, position, orientation, delay=0, waypoints=[], corrections=1, action_server=None):
if len(waypoints) > 0:
# this is good for doing gestures
plan, fraction = self.arm.compute_cartesian_path(waypoints, eef_step=0.01, jump_threshold=0.0)
else:
p = self.arm.get_current_pose()
p.pose.position = position
p.pose.orientation = orientation
self.arm.set_pose_target(p)
plan = self.arm.plan()
if plan:
# get the last pose to correct if desired
ptPos = plan.joint_trajectory.points[-1].positions
# print "=================================="
# print "Last point of the current trajectory: "
angle_set = list()
for i in range(len(ptPos)):
tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
angle_set.append(tempPos)
if action_server:
action_server.publish_feedback(CalibrateFeedback("Plan Found"))
self.arm.execute(plan, wait=False)
while self.move_group_state is not "IDLE":
rospy.sleep(0.001)
print(self.move_group_state)
if self.paused is True:
self.arm.stop()
return
rospy.loginfo("LEAVING THE WHILE LOOP")
print("LEAVING THE LOOOOOOOOOP!!!!")
if corrections > 0:
rospy.loginfo("Correcting the pose")
self.move_joint_angles(angle_set)
rospy.sleep(delay)
else:
if action_server:
action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
# Let's have the caller handle sequences instead.
# def handle_move_sequence(self, message):
# # if the move fails, do we cancel the sequence
# cancellable = message.cancellable
# moves = message.moves
# for move in moves:
# try:
# self.handle_move_block(move)
# except Exception:
# if cancellable:
# rospy.loginfo("Part of move failed, cancelling the rest.")
# break
def update_move_group_state(self, message):
rospy.loginfo(message.feedback.state)
self.move_group_state = message.feedback.state
def get_grasp_orientation(self, position):
return Quaternion(1, 0, 0, 0)
def update_joints(self, joints):
self.joint_angles = [joints.joint1, joints.joint2, joints.joint3,
joints.joint4, joints.joint5, joints.joint6, joints.joint7]
def move_z(self, distance):
p = self.arm.get_current_pose()
p.pose.position.z += distance
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def pick_block(self, location, check_grasp=False, retry_attempts=0, delay=0, action_server=None):
# go to a spot and pick up a block
# if check_grasp is true, it will check torque on gripper and retry or fail if not holding
# open the gripper
self.open_gripper()
position = Point(location.x, location.y, self.grasp_height)
orientation = self.get_grasp_orientation(position)
try:
self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server)
except Exception as e:
raise(e)
if action_server:
action_server.publish_feedback()
if check_grasp is True:
pass # for now, but we want to check finger torques
# for i in range(retry_attempts):
# self.move_z(0.3)
self.close_gripper()
self.move_z(0.1)
rospy.sleep(delay)
def place_block(self, location, check_grasp=False, delay=0, action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
position = Point(location.x, location.y, self.drop_height)
orientation = self.get_grasp_orientation(position)
try:
self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server)
except Exception as e:
raise(e)
if action_server:
action_server.publish_feedback()
self.open_gripper()
self.move_z(0.1)
self.close_gripper()
def stop_motion(self, home=False, pause=False):
rospy.loginfo("STOPPING the ARM")
# cancel the moveit_trajectory
# self.arm.stop()
# call the emergency stop on kinova
# self.emergency_stop()
# rospy.sleep(0.5)
if pause is True:
self.paused = True
if home is True:
# self.restart_arm()
self.home_arm()
def calibrate(self, message):
print("calibrating ", message)
self.place_calibration_block()
rospy.sleep(5) # five seconds to correct the drop if it bounced, etc.
self.calibration_server.publish_feedback(CalibrateFeedback("getting the coordinates"))
params = self.record_calibration_params()
self.set_arm_calibration_params(params[0], params[1])
self.calibration_publisher.publish(CalibrationParams(params[0], params[1]))
self.calibration_server.set_succeeded(CalibrateResult(params))
def set_arm_calibration_params(self, arm_x_min, arm_y_min):
rospy.set_param("ARM_X_MIN", arm_x_min)
rospy.set_param("ARM_Y_MIN", arm_y_min)
def place_calibration_block(self):
# start by homing the arm (kinova home)
self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
self.home_arm_kinova()
# go to grab a block from a human
self.open_gripper()
self.close_gripper()
self.open_gripper(4)
self.close_gripper(2)
# move to a predetermined spot
self.calibration_server.publish_feedback(CalibrateFeedback("moving to drop"))
self.move_arm_to_pose(Point(0.4, -0.4, 0.1), Quaternion(0, 1, 0, 0))
# drop the block
self.open_gripper()
self.calibration_server.publish_feedback(CalibrateFeedback("dropped the block"))
calibration_block = {'x': 0.4, 'y': 0.4, 'id': 0}
calibration_action_belief = {"action": "add", "block": calibration_block}
self.action_belief_publisher.publish(String(json.dumps(calibration_action_belief)))
rospy.loginfo("published arm belief")
return
def record_calibration_params(self):
""" Call the block_tracker service to get the current table config.
Find the calibration block and set the appropriate params.
"""
# make sure we know the dimensions of the table before we start
# fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
tuio_bounds = get_tuio_bounds()
arm_bounds = get_arm_bounds(calibrate=False)
try:
block_state = json.loads(self.get_block_state("tuio").tui_state)
except rospy.ServiceException as e:
# self.calibration_server.set_aborted()
raise(ValueError("Failed getting block state to calibrate: "+str(e)))
if len(block_state) != 1:
# self.calibration_server.set_aborted()
raise(ValueError("Failed calibration, either couldn't find block or > 1 block on TUI!"))
# if we made it here, let's continue!
# start by comparing the reported tuio coords where we dropped the block
# with the arm's localization of the end-effector that dropped it
# (we assume that the block fell straight below the drop point)
drop_pose = self.arm.get_current_pose()
end_effector_x = drop_pose.pose.position.x
end_effector_y = drop_pose.pose.position.y
block_tuio_x = block_state[0]['x']
block_tuio_y = block_state[0]['y']
x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y, tuio_bounds)
arm_x_min = end_effector_x - x_ratio*arm_bounds["x_dist"]
arm_y_min = end_effector_y - y_ratio*arm_bounds["y_dist"]
return [arm_x_min, arm_y_min]
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.40
p.pose.position.y = -0.0
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.37636
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()
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
#rospy.logwarn("cleaning world")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("open")
#right_gripper.go()
rospy.sleep(3)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
p.pose.position.x = 1.0
p.pose.position.y = 0.2
p.pose.position.z = 0.3
scene.add_box("table", p, (0.7, 1, 0.7))
# add an object to be grasped
p.pose.position.x = 0.4
p.pose.position.y = 0
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# add a position for placement
p1 = PoseStamped()
class CreateScene2(object):
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 msg_cb(self, msg):
global first_stamp, now
if self.first_stamp is None:
now = rospy.Time.now()
first_stamp = msg.header.stamp
msg.header.stamp -= first_stamp
msg.header.stamp += now
for i in range(3):
self.cloud_pub.publish(msg)
def add_box_object(self, name, dimensions, pose):
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = pose[0]
p.pose.position.y = pose[1]
p.pose.position.z = pose[2]
p.pose.orientation.x = pose[3]
p.pose.orientation.y = pose[4]
p.pose.orientation.z = pose[5]
p.pose.orientation.w = pose[6]
self._scene.add_box(name, p,
(dimensions[0], dimensions[1], dimensions[2]))
rospy.sleep(0.2)
def spin(self, cloud_topic):
tf_broadcaster = tf.TransformBroadcaster()
rate = rospy.Rate(20)
while not rospy.is_shutdown():
tf_broadcaster.sendTransform(
(self._kinect_pose_1[0], self._kinect_pose_1[1],
self._kinect_pose_1[2]),
(self._kinect_pose_1[3], self._kinect_pose_1[4],
self._kinect_pose_1[5], self._kinect_pose_1[6]),
rospy.Time.now(), cloud_topic, "world")
rate.sleep()
class UR5_Gripped_Manipulator:
"""
Class Attributes
Robot : Robot Commander Object
Scene : Planning Scene Interface Object (current scene)
Man : MoveGroupCommander object to control manipulator
EEF : MoveGroupCommander object to control endeffector
Target_poses : Pose list of created target objects
Picked : List of part indexes to ensure to picked non-picked ones
"""
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 = []
# self.client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)
self.client = actionlib.SimpleActionClient(
'gripper_controller/gripper_action', FollowJointTrajectoryAction)
# self.client.wait_for_server()
if not self.client.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
"""
----------------Function Name: clean_scene----------------
Definition: Clears the target_poses and picked lists in order to establish a new execution. Necessary for test_simulation method
"""
def clear_poses(self):
for i in xrange(len(self.target_poses)):
self.target_poses.pop()
for i in xrange(len(self.picked)):
self.picked.pop()
"""
----------------Function Name: clean_scene----------------
Definition: Clears all non-attached objects from planning scene
"""
def clean_scene(self):
rospy.loginfo("Clearing the Scene")
self.scene.remove_world_object()
"""
----------------Function Name: add_object----------------
Name : Object Name
Pose : Pose of the Object (x,y,z,xo,yo,zo,wo)
Dimension : Dimensions of the Obhect (Tuple) (d1,d2,d3)
Type : Box(0),Sphere(1)
d1 is radius for sphere i.e typ==1,
"""
def add_object(self,
name,
x,
y,
z,
xo=0.0,
yo=0.0,
zo=0.0,
wo=0.0,
d1=0.1,
d2=0.1,
d3=0.1,
typ=0):
pos = PoseStamped()
pos.header.frame_id = self.robot.get_planning_frame()
pos.pose.position.x = x
pos.pose.position.y = y
pos.pose.position.z = z
pos.pose.orientation.x = xo
pos.pose.orientation.y = yo
pos.pose.orientation.z = zo
pos.pose.orientation.w = wo
rospy.loginfo("ADDING OBJECT")
if (typ == 0):
self.scene.add_box(name, pos, (d1, d2, d3))
elif (typ == 1):
self.scene.add_sphere(name, pos, d1)
else:
print("ERROR in Type")
return pos
"""
----------------Function Name: create_environment----------------
Definition : Creates the simulation environment with non-target collision objects, can be edited to create different default environment
"""
def create_environment(self):
# self.add_object(name="wall",x=0.0,y=0.8,z=0.5,d1=0.1,d2=0.35,d3=1,typ=0)
# self.add_object(name="wall_2",x=0.0,y=-0.8,z=0.5,d1=0.1,d2=0.35,d3=1,typ=0)
self.add_object(name="table",
x=0.0,
y=0.0,
z=-0.05,
d1=2,
d2=2,
d3=0.0001,
typ=0)
"""
----------------Function Name: check_targets----------------
Definition : Creates given number of objects within the distance provided. It prevents collision object overlapping.
It returns the pose list of the objects
Number : Number of collision objects required to spawn
Distance : Required minimum distance between each collision objects
"""
def check_targets(self, number, distance):
rospy.loginfo("Creating Boxes!")
def al(typ, x=0.0):
if typ == 'x':
return random.uniform(0.35, 0.65)
elif typ == "y":
rang = math.sqrt(0.8**2 - x**2)
return random.uniform(-rang, rang)
hemme = []
dims = []
cnt = 0
while len(hemme) != number:
if cnt == 200:
hemme = []
cnt = 0
cnt = cnt + 1
# dim = (random.uniform(0.08,0.12),random.uniform(0.08,0.10),random.uniform(0.05,0.2))
# quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0,random.uniform(-math.pi ,math.pi))
dim = (0.09, 0.09, 0.2)
box = PoseStamped()
box.header.frame_id = self.robot.get_planning_frame()
box.pose.position.z = (-0.04 + (dim[2] / 2))
# box.pose.position.x = al('x')
# box.pose.position.y = al('y',box.pose.position.x)
# box.pose.orientation.x = quaternion[0]
# box.pose.orientation.y = quaternion[1]
# box.pose.orientation.z = quaternion[2]
# box.pose.orientation.w = quaternion[3]
box.pose.position.x = 0.35
box.pose.position.y = 0.35
box.pose.position.z = 0.05
box.pose.orientation.x = 0
box.pose.orientation.y = 0
box.pose.orientation.z = 0
box.pose.orientation.w = 1
flag = 1
for i in hemme:
if abs(i.pose.position.x -
box.pose.position.x) < distance or abs(
i.pose.position.y - box.pose.position.y) < distance:
flag = 0
if flag == 0:
continue
hemme.append(box)
dims.append(dim)
cnt = 0
names = []
for i in xrange(len(hemme)):
now = "part" + str(cnt)
cnt = cnt + 1
names.append(now)
self.add_object(name=now,
x=hemme[i].pose.position.x,
y=hemme[i].pose.position.y,
z=hemme[i].pose.position.z,
xo=hemme[i].pose.orientation.x,
yo=hemme[i].pose.orientation.y,
zo=hemme[i].pose.orientation.z,
wo=hemme[i].pose.orientation.w,
d1=dims[i][0],
d2=dims[i][1],
d3=dims[i][2],
typ=0)
print("End Check!")
rospy.loginfo(hemme)
return hemme
"""
----------------Function Name: clean_scene----------------
Definition : Clears all non-attached objects from planning scene
Group : MoveGroupCommander object to control given group
"""
def default_state_gripper(self, grp):
rospy.loginfo("Openning Gripper")
joint_vals = grp.get_current_joint_values()
joint_vals[0] = 0.0
grp.set_joint_value_target(joint_vals)
init_plan = grp.plan()
return grp.execute(init_plan)
"""
----------------Function Name: closed_state_gripper----------------
Definition : Function that opens gripper and detachs the gripped object
Obj : Name of the Object that is needed to detach
"""
def closed_state_gripper(self, obj):
rospy.loginfo("Closing Gripper")
def convert(width):
return 0.77 - width / 0.15316 * 0.77
width = self.scene.get_objects([obj])[obj].primitives[0].dimensions[0]
width = convert(width)
now = self.robot.endeffector.get_current_joint_values()[0]
cnt = 0
while abs(now - width) > 0.0001:
now = self.robot.endeffector.get_current_joint_values()[0]
cnt = cnt + 1
tmp = width - abs(now - width) / 2.0
self.robot.endeffector.set_joint_value_target('finger_joint', tmp)
pl = self.robot.endeffector.plan()
# self.robot.endeffector.go()
self.client.wait_for_server()
goal = FollowJointTrajectoryAction().action_goal.goal
goal.trajectory = pl.joint_trajectory
rospy.loginfo(goal)
# goal=GripperCommandAction().action_goal.goal
# goal.command.position = tmp
# goal.command.max_effort=1
state = self.client.send_goal_and_wait(goal)
rospy.loginfo(state)
if state == 0:
rospy.logerr('Pick up goal failed!: %s' %
self.client.get_goal_status_text())
return None
rospy.sleep(0.05)
if cnt == 7:
break
rospy.sleep(1.0)
ret = self.robot.manipulator.attach_object(obj)
return ret
"""
----------------Function Name: pick_object----------------
Definition : It moves the given group i.e robot to the collision object whose index is given and picks that object.
Group : MoveGroupCommander object to control given group
Part_index : Index of the target object to obtain pose
"""
def pick_object(self, group, part_index):
rospy.loginfo("Pick Operation starts!")
gripped_object = scene.get_objects(["part" + str(part_index)
])["part" + str(part_index)]
pos = copy.deepcopy(self.target_poses[part_index])
rospy.loginfo(pos)
temp = tf.transformations.euler_from_quaternion(
(pos.pose.orientation.x, pos.pose.orientation.y,
pos.pose.orientation.z, pos.pose.orientation.w))
quaternion = tf.transformations.quaternion_from_euler(
math.pi, temp[1], temp[2])
# pos.pose.position.z += 0.17 + (gripped_object.primitives[0].dimensions[2]/2.0)
pos.pose.position.z += 0.14 + (
gripped_object.primitives[0].dimensions[2] / 2.0)
# pos.pose.orientation.x = quaternion[0]
# pos.pose.orientation.y = quaternion[1]
# pos.pose.orientation.z = quaternion[2]
# pos.pose.orientation.w = quaternion[3]
pos.pose.orientation.x = 0
pos.pose.orientation.y = 0.707
pos.pose.orientation.z = 0
pos.pose.orientation.w = 0.707
rospy.loginfo(pos)
group.set_pose_target(pos)
move_plan = group.plan()
i = 0
while (not move_plan.joint_trajectory.joint_names):
move_plan = group.plan()
i += 1
if (i == 500): break
state = group.execute(move_plan)
rospy.loginfo("Execute operation for Object is %s" % str(part_index))
if (state):
self.closed_state_gripper("part" + str(part_index))
rospy.sleep(1.0)
self.place_object(group, part_index)
return
else:
return
"""
----------------Function Name: pick_object----------------
Definition : It places the gripped object to the target location
Group : MoveGroupCommander object to control given group
Part_index : Index of the target object to obtain pose
"""
def place_object(self, group, part_index):
def al(typ, x=0.0):
if typ == 'x':
return random.uniform(-0.35, -0.6)
elif typ == 'y':
rang = math.sqrt(0.75**2 - x**2)
x = random.uniform(-rang, rang)
return x
pos = PoseStamped()
pos.header.frame_id = self.robot.get_planning_frame()
pos.pose.position.x = al("x")
pos.pose.position.y = al("y", pos.pose.position.x)
group.set_pose_target(pos)
#This line makes placing possible by setting a valid z position for gripped object
pos.pose.position.z = -0.04 + 0.17 + ((scene.get_attached_objects([
"part" + str(part_index)
])["part" + str(part_index)]).object.primitives[0].dimensions[2])
pos.pose.orientation.y = 1.0
group.set_pose_target(pos)
move_plan = group.plan()
state = group.execute(move_plan)
rospy.loginfo("placed")
if (state):
detached = group.detach_object("part" + str(part_index))
rospy.sleep(1.5)
if (detached):
self.scene.remove_world_object("part" + str(part_index))
rospy.sleep(1.5)
self.default_state_gripper(self.eef)
self.picked.append(part_index)
else:
self.default_state_gripper(self.robot.endeffector)
group.detach_object("part" + str(part_index))
rospy.sleep(2)
"""
----------------Function Name: execute_simulation----------------
Definition : Creates the specified environment , generates motion plans and does the pick and place operation based on these plans. To visualize RViz is needed.
Planner ID can be changed based on MoveIT's supported OMPL.
"""
def execute_simulation(self, num_of_objects=1):
is_success = False
#Reset State of the Gripper
self.default_state_gripper(self.eef)
####
#Reset the position of the Arm
# Will be implemented if needed
####
#Clean the scene
self.clean_scene()
rospy.sleep(1.0)
#Create environment
self.create_environment()
#Create targets
self.target_poses = self.check_targets(num_of_objects, 0.10)
rospy.sleep(5)
#Planner setup
self.man.set_planner_id("RRTConnectkConfigDefault")
self.man.set_num_planning_attempts(20)
self.man.allow_replanning(True)
#Pick and place every object
for i in xrange(len(self.target_poses)):
if i not in self.picked:
self.pick_object(group=self.man, part_index=i)
rospy.loginfo("END OF PICK AND PLACE")
if (len(self.picked) == len(self.target_poses)):
is_success = True
self.clear_poses()
return is_success
"""
----------------Function Name: execute_simulation----------------
Definition : Executes the simulation num_attempts times. Stores the success rate
and writes into the specified document
Num_Attempts : Limiting number for executing simulation. (Default Value: 10)
File_Name : File name (with path if needed) to write test results
"""
def test_simulation(self,
num_attempts=10,
num_of_objects=1,
file_name="ur5_pick_place_test.txt"):
success_rate = 0
file = open(file_name, "a+")
file.write("Start Test \n")
for case in xrange(1, num_attempts + 1):
state = self.execute_simulation(num_of_objects=num_of_objects)
if (state):
success_rate += 1
rospy.loginfo("Success Rate: " + str(success_rate) + "/" +
str(case))
file.write("Test Case %s is successfull" % str(case))
else:
rospy.loginfo("Success Rate: " + str(success_rate) + "/" +
str(case))
file.write("Test Case %s is failedl" % str(case))
file.write("Success: %" + str(((success_rate / (case - 1) * 100.0))))
file.write("----END OF TEST----")
file.close()
return
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)
class Pick_Place:
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_distrost', 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(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_grasp_block_(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.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))
# 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.45
p.pose.position.y = 0.0
p.pose.position.z = 0.0
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_grasp_block_(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.05
p.pose.position.z = 0.1
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.03, 0.03, 0.09))
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 = 0.2
# 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 = 0.2
# 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)
class Pick_Place:
def __init__(self, x, y, z):
self._x = x
self._y = y
self._z = z
# 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() #未知
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name,
self._x, self._y, self._z)
rospy.sleep(0.5)
# Define target place pose:定义目标位置姿势
self._pose_place = Pose()
self._pose_place.position.x = 0 #self._pose_coke_can.position.x
self._pose_place.position.y = -0.85 #self._pose_coke_can.position.y - 0.20
self._pose_place.position.z = 0.7 #self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 #self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above
self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
self._arm.set_named_target('up')
self._arm.go(wait=True)
print("up pose")
print("第一部分恢复位置初始")
# Create grasp generator 'generate' action client:
# #创建抓取生成器“生成”动作客户端：
print("开始Action通信")
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
print("结束Action通信")
# Create move group 'pickup' action client:
# 创建移动组“抓取”操作客户端：
print("开始pickup通信")
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
print("结束pickup通信")
# Create move group 'place' action client:
# 创建移动组“放置”动作客户端：
print("开始place通信")
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
print("结束place通信")
# 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(0.5)
print("抓取物体")
rospy.loginfo('Pick up successfully')
print("pose_place: ", self._pose_place)
# 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(0.5)
rospy.loginfo('Place successfully')
def _generate_grasps(self, pose, width):
"""
使用抓握生成器生成动作来生成抓握； 基于moveit_simple_grasps pkg上的server_test.py示例。
生成抓取姿势阵列数据以进行可视化，
然后将抓取目标发送到抓取服务器.
"""
self._grasps_ac.wait_for_server()
rospy.loginfo("Successfully connected!")
# 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:
# Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary.
#
rospy.loginfo("Sent goal,waiting,目标物体的位置:\n " + str(goal))
#self._grasps_ac.wait_for_result()
#发送目标并等待结果：
# ＃将目标发送到ActionServer，等待目标完成，然后抢占目标是必要的。
t_start = rospy.Time.now()
state = self._grasps_ac.send_goal_and_wait(goal)
t_end = rospy.Time.now()
t_toal = t_end - t_start
rospy.loginfo("发送时间Result received in " + str(t_toal.to_sec()))
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)
"""
print("---------grasps--start------------")
print(grasps)
print("-----------test-grasps-----end-----------------")
rospy.sleep(2)
print("-grasps---sleep----end-----")
"""
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()
#print("---------------test3--------------------------")
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 = 0.1
#print("place1---test=====")
# 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 = 0.06
#print("place2---test=====")
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
"""
print(places)
print("------test4---------------------------------------")
"""
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
Create a goal for picking up the grasping object创建一个捡起抓取物体的目标
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group #规划的组
goal.target_name = target #要拾取的对象的名称（
#可能使用的抓握列表。 必须至少填写一个掌握
goal.possible_grasps.extend(grasps)
#障碍物的可选清单，我们掌握有关语义的信息，可以在抓取过程中触摸/推动/移动这些障碍物； 注意：如果使用对象名称“ all”，则在进近和提升过程中禁止与所有对象发生碰撞。
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
"""
print("----goal-------test-----")
print(goal)
print("----goal---end------test---")
"""
rospy.sleep(0.5)
print("-goal---sleep----end-----")
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
Create a place goal for MoveIt!
"""
# 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 _add_grasp_block_(self, name, _x, _y, _z):
"""
Create and add block to the scene创建场景并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = _x
p.pose.position.y = _y
p.pose.position.z = _z
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.045, 0.045, 0.045))
return p.pose
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)
#print("--goal--start----")
# Create and send Pickup goal:创建并发送zhua取目标：
goal = self._create_pickup_goal(group, target, grasps)
#print("--goal--end----")
#print("---pick---up---1----")
state = self._pickup_ac.send_goal_and_wait(goal)
#print("-----------------state-------------------")
#print(state)
#print("---pick---up---2----")
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()
"""
print("-----test1--------")
print(result)
print("------test1--end------")
"""
# Check for error:
err = result.error_code.val
print(err)
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
print("pickup-----end-----")
rospy.sleep(0.5)
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()
"""
print("-----test2--------")
print(result)
print("------test2--end------")
"""
# 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使用PoseArray消息将抓取发布为姿势
"""
print(self._grasps_pub.get_num_connections())
if self._grasps_pub.get_num_connections() != 1:
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))
print(msg)
rospy.loginfo('Publisher ' + str(len(msg.poses)) + ' poses')
self._grasps_pub.publish(msg)
"""
print("11-111")
rospy.loginfo('Publisher'+str(len(msg))+'poses')
print("11-111")
rospy.sleep(2)
"""
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)
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
scene.add_box("block2", p, (0.044, 0.044, 0.044))
p.pose.position.x = 0.6
p.pose.position.y = -0.7
p.pose.position.z = 0.5
scene.add_box("block3", p, (0.044, 0.044, 0.044))
p.pose.position.x = 1
p.pose.position.y = -0.7
p.pose.position.z = 0.5
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)
