def commander():
global past_reads
rospy.init_node('rubik_camera', anonymous=True)
rospy.Subscriber("/usb_cam/image_raw", Image, callback)
pub = rospy.Publisher("cube_configuration", String, queue_size=1)
# Gets a handle on the robot that MoveIt is controlling
robot = RobotCommander()
rospy.sleep(1)
# Picks a group called "manipulator" to work with #
arm = robot.get_group("manipulator")
arm.set_max_velocity_scaling_factor(1.0)
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.005)
go_home(arm)
pick_rubik(arm, robot, "right")
go_home(arm)
for i in range(5):
read(arm, i)
go_home(arm)
place_rubik(arm, robot, "right")
go_home(arm)
pick_rubik(arm, robot, "left")
go_home(arm)
for i in range(5):
read(arm, i)
go_home(arm)
place_rubik(arm, robot, "left")
go_home(arm)
print(str(past_reads))
pub.publish(reads_to_cube())
rospy.sleep(1)
class PythonTimeParameterizationTest(unittest.TestCase):
PLANNING_GROUP = "arm"
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def plan(self):
start_pose = self.group.get_current_pose().pose
goal_pose = self.group.get_current_pose().pose
goal_pose.position.z -= 0.1
(plan,
fraction) = self.group.compute_cartesian_path([start_pose, goal_pose],
0.005, 0.0)
self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
return plan
def time_parameterization(self, plan):
ref_state = self.commander.get_current_state()
retimed_plan = self.group.retime_trajectory(
ref_state, plan, velocity_scaling_factor=0.1)
return retimed_plan
def test_plan_and_time_parameterization(self):
plan = self.plan()
retimed_plan = self.time_parameterization(plan)
class PythonTimeParameterizationTest(unittest.TestCase):
PLANNING_GROUP = "manipulator"
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def plan(self):
start_pose = self.group.get_current_pose().pose
goal_pose = self.group.get_current_pose().pose
goal_pose.position.z -= 0.1
(plan, fraction) = self.group.compute_cartesian_path(
[start_pose, goal_pose], 0.005, 0.0
)
self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
return plan
def time_parameterization(self, plan, algorithm):
ref_state = self.commander.get_current_state()
retimed_plan = self.group.retime_trajectory(
ref_state,
plan,
velocity_scaling_factor=0.1,
acceleration_scaling_factor=0.1,
algorithm=algorithm,
)
return retimed_plan
def test_plan_and_time_parameterization(self):
plan = self.plan()
retimed_plan = self.time_parameterization(
plan, "iterative_time_parameterization"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(
plan, "iterative_spline_parameterization"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(
plan, "time_optimal_trajectory_generation"
)
self.assertTrue(
len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid"
)
retimed_plan = self.time_parameterization(plan, "")
self.assertTrue(
len(retimed_plan.joint_trajectory.points) == 0, "Invalid retime algorithm"
)
class PythonMoveitCommanderNsTest(unittest.TestCase):
PLANNING_GROUP = "manipulator"
PLANNING_NS = "test_ns/"
@classmethod
def setUpClass(self):
self.commander = RobotCommander(
"%srobot_description" % self.PLANNING_NS, self.PLANNING_NS)
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def check_target_setting(self, expect, *args):
if len(args) == 0:
args = [expect]
self.group.set_joint_value_target(*args)
res = self.group.get_joint_value_target()
self.assertTrue(
np.all(np.asarray(res) == np.asarray(expect)),
"Setting failed for %s, values: %s" % (type(args[0]), res),
)
def test_target_setting(self):
n = self.group.get_variable_count()
self.check_target_setting([0.1] * n)
self.check_target_setting((0.2, ) * n)
self.check_target_setting(np.zeros(n))
self.check_target_setting(
[0.3] * n, {name: 0.3
for name in self.group.get_active_joints()})
self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
def plan(self, target):
self.group.set_joint_value_target(target)
return self.group.plan()
def test_validation(self):
current = np.asarray(self.group.get_current_joint_values())
success1, plan1, time1, err1 = self.plan(current + 0.2)
success2, plan2, time2, err2 = self.plan(current + 0.2)
self.assertTrue(success1)
self.assertTrue(success2)
# first plan should execute
self.assertTrue(self.group.execute(plan1))
# second plan should be invalid now (due to modified start point) and rejected
self.assertFalse(self.group.execute(plan2))
# newly planned trajectory should execute again
success3, plan3, time3, err3 = self.plan(current)
self.assertTrue(success3)
self.assertTrue(self.group.execute(plan3))
class PythonMoveitCommanderTest(unittest.TestCase):
PLANNING_GROUP = "manipulator"
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def check_target_setting(self, expect, *args):
if len(args) == 0:
args = [expect]
self.group.set_joint_value_target(*args)
res = self.group.get_joint_value_target()
self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)),
"Setting failed for %s, values: %s" % (type(args[0]), res))
def test_target_setting(self):
n = self.group.get_variable_count()
self.check_target_setting([0.1] * n)
self.check_target_setting((0.2,) * n)
self.check_target_setting(np.zeros(n))
self.check_target_setting([0.3] * n, {name: 0.3 for name in self.group.get_active_joints()})
self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
def plan(self, target):
self.group.set_joint_value_target(target)
return self.group.plan()
def test_validation(self):
current = np.asarray(self.group.get_current_joint_values())
success1, plan1, time1, err1 = self.plan(current + 0.2)
success2, plan2, time2, err2 = self.plan(current + 0.2)
self.assertTrue(success1)
self.assertTrue(success2)
# first plan should execute
self.assertTrue(self.group.execute(plan1))
# second plan should be invalid now (due to modified start point) and rejected
self.assertFalse(self.group.execute(plan2))
# newly planned trajectory should execute again
success3, plan3, time3, err3 = self.plan(current)
self.assertTrue(success3)
self.assertTrue(self.group.execute(plan3))
def test_planning_scene_interface(self):
planning_scene = PlanningSceneInterface()
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)
class PythonMoveitCommanderTest(unittest.TestCase):
PLANNING_GROUP = "manipulator"
JOINT_NAMES = [
"joint_1",
"joint_2",
"joint_3",
"joint_4",
"joint_5",
"joint_6",
]
@classmethod
def setUpClass(self):
self.commander = RobotCommander("robot_description")
self.group = self.commander.get_group(self.PLANNING_GROUP)
@classmethod
def tearDown(self):
pass
def test_enforce_bounds_empty_state(self):
in_msg = RobotState()
with self.assertRaises(genpy.DeserializationError):
self.group.enforce_bounds(in_msg)
def test_enforce_bounds(self):
in_msg = RobotState()
in_msg.joint_state.header.frame_id = "base_link"
in_msg.joint_state.name = self.JOINT_NAMES
in_msg.joint_state.position = [0] * 6
in_msg.joint_state.position[0] = 1000
out_msg = self.group.enforce_bounds(in_msg)
self.assertEqual(in_msg.joint_state.position[0], 1000)
self.assertLess(out_msg.joint_state.position[0], 1000)
def test_get_current_state(self):
expected_state = RobotState()
expected_state.joint_state.header.frame_id = "base_link"
expected_state.multi_dof_joint_state.header.frame_id = "base_link"
expected_state.joint_state.name = self.JOINT_NAMES
expected_state.joint_state.position = [0] * 6
self.assertEqual(self.group.get_current_state(), expected_state)
def check_target_setting(self, expect, *args):
if len(args) == 0:
args = [expect]
self.group.set_joint_value_target(*args)
res = self.group.get_joint_value_target()
self.assertTrue(
np.all(np.asarray(res) == np.asarray(expect)),
"Setting failed for %s, values: %s" % (type(args[0]), res),
)
def test_target_setting(self):
n = self.group.get_variable_count()
self.check_target_setting([0.1] * n)
self.check_target_setting((0.2,) * n)
self.check_target_setting(np.zeros(n))
self.check_target_setting(
[0.3] * n, {name: 0.3 for name in self.group.get_active_joints()}
)
self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
def plan(self, target):
self.group.set_joint_value_target(target)
return self.group.plan()
def test_validation(self):
current = np.asarray(self.group.get_current_joint_values())
success1, plan1, time1, err1 = self.plan(current + 0.2)
success2, plan2, time2, err2 = self.plan(current + 0.2)
self.assertTrue(success1)
self.assertTrue(success2)
# first plan should execute
self.assertTrue(self.group.execute(plan1))
# second plan should be invalid now (due to modified start point) and rejected
self.assertFalse(self.group.execute(plan2))
# newly planned trajectory should execute again
success3, plan3, time3, err3 = self.plan(current)
self.assertTrue(success3)
self.assertTrue(self.group.execute(plan3))
def test_gogogo(self):
current_joints = np.asarray(self.group.get_current_joint_values())
self.group.set_joint_value_target(current_joints)
self.assertTrue(self.group.go(True))
self.assertTrue(self.group.go(current_joints))
self.assertTrue(self.group.go(list(current_joints)))
self.assertTrue(self.group.go(tuple(current_joints)))
self.assertTrue(
self.group.go(JointState(name=self.JOINT_NAMES, position=current_joints))
)
self.group.remember_joint_values("current")
self.assertTrue(self.group.go("current"))
current_pose = self.group.get_current_pose()
self.assertTrue(self.group.go(current_pose))
def test_planning_scene_interface(self):
planning_scene = PlanningSceneInterface()
class EdPick:
def __init__(self):
allowed_planning_time = 0.0
ARM_AND_LINEAR_GROUP_NAME = "arm_and_linear"
ARM_GROUP_NAME = "arm_torso"
GRIPPER_GROUP = "gripper"
REFERENCE_FRAME = "base_link"
PLANNER = "RRTConnectkConfigDefault"
#PLANNER = "RRTstarkConfigDefault"
MOVE_GROUP_SERVICE = "/move_group/plan_execution/set_parameters"
result = False
upright_constraints = None
robot = None
arm = None
arm_and_linear = None
database = None
scene = None
grasp_generator = None
place_generator = None
marker_publisher = None
self.robot = RobotCommander()
self.arm = self.robot.get_group("arm_torso")
self.gripper = self.robot.get_group(GRIPPER_GROUP)
self.scene = PlanningSceneInterface()
self.pickup_object = RecognizedObject("part")
self.table = EntityInfo()
def get_object(self):
rospy.wait_for_service('/ed/kinect/update')
self.update_client = rospy.ServiceProxy('/ed/kinect/update', Update)
self.request_update = UpdateRequest()
self.request_update.area_description = "on_top_of dinner-table"
self.result_update = self.update_client(self.request_update)
print "Found IDs: " % self.result_update.new_ids
rospy.wait_for_service('/ed/simple_query')
self.query_client = rospy.ServiceProxy('/ed/simple_query', SimpleQuery)
self.query_request = SimpleQueryRequest()
self.query_request.id = self.result_update.new_ids[0]
self.query_response = self.query_client(self.query_request)
rospy.wait_for_service('/ed/simple_query')
self.ed_simple = rospy.ServiceProxy('/ed/simple_query', SimpleQuery)
simpleRequest = SimpleQueryRequest()
simpleRequest.id = 'dinner-table'
self.table = self.ed_simple(simpleRequest).entities[0]
print self.table.pose
def pick_object(self):
# self.object = self.query_response.entities
self.pickup_object = RecognizedObject("part")
tmpPoseStamped = PoseStamped()
tmpPoseStamped.header.frame_id = '/map'
tmpPoseStamped.header.stamp = rospy.Time(0)
tmpPoseStamped.pose = self.query_response.entities[0].pose
self.pickup_obj.pose = tmpPoseStamped
self.value_x = []
self.value_y = []
self.value_z = []
for j in range(0, len(self.query_response.entities[0].convex_hull)):
self.value_x.append(
self.query_response.entities[0].convex_hull[j].x)
self.value_y.append(
self.query_response.entities[0].convex_hull[j].y)
self.value_z.append(
self.query_response.entities[0].convex_hull[j].z)
self.dim_x = max(self.value_x) - min(self.value_x)
self.dim_y = max(self.value_y) - min(self.value_y)
self.dim_z = max(self.value_z) - min(self.value_z)
self.pickup_object.dimensions = (self.dim_x, self.dim_y, self.dim_z)
self.add_to_planning_scene(self.pickup_object)
self.grasps = self.generate_grasps_for_object(self.pickup_object)
if not self.pickup(self.pickup_object, self.grasps) == -1:
print "Executed pick action"
self.attach_object_to_gripper(self.pickup_object)
self.place_location.header = tmpPoseStamped.header
self.place_location.pose.position.x = self.result.entityInfos[
i].pose.position.x
self.place_location.pose.position.y = self.result.entityInfos[
i].pose.position.y + 0.1
self.place_location.pose.position.z = self.result.entityInfos[
i].pose.position.z
self.place_location.pose.orientation.x = self.result.entityInfos[
i].pose.orientation.x
self.place_location.pose.orientation.y = self.result.entityInfos[
i].pose.orientation.y
self.place_location.pose.orientation.z = self.result.entityInfos[
i].pose.orientation.z
self.place_location.pose.orientation.w = self.result.entityInfos[
i].pose.orientation.w
myTable = MyTable(tmpPoseStamped.header, self.table.pose,
self.table.convex_hull)
self.place(self.recog_obj, myTable)
print "Executed place action"
def add_to_planning_scene(self, recognized_object):
pose_stamped = copy.deepcopy(recognized_object.pose)
self.scene.add_box(
recognized_object.name, pose_stamped,
(recognized_object.dimensions[0], recognized_object.dimensions[1],
recognized_object.dimensions[2]))
rospy.loginfo("Object {} added to planning scene".format(
recognized_object.name))
rospy.logdebug(
"Object {} added with pose \n{} \nand dimensions {}".format(
recognized_object.name, pose_stamped,
recognized_object.dimensions))
def attach_object_to_gripper(self, recognized_object):
rospy.loginfo("Attach object to gripper")
pose_stamped = copy.deepcopy(recognized_object.pose)
# self.remove_attached_object(recognized_object.name)
self.scene.remove_world_object(recognized_object.name)
self.scene.attach_box(
END_EFFECTOR_LINK_NAME, recognized_object.name, pose_stamped,
(recognized_object.dimensions[0], recognized_object.dimensions[1],
recognized_object.dimensions[2]),
GRIPPER_LINKS_TO_IGNORE_COLLISION)
def pickup(self, pickup_object, grasps):
supp_surface = 'table',
rospy.loginfo("Recognized object: " + str(pick_obj))
self.arm.set_support_surface_name(supp_surface)
result = self.arm.pick(pick_object.name, grasps)
self.print_moveit_result(result)
return result
def generate_grasps_for_object(self, recognized_object):
self.recognized_object = recognized_object
self.base_grasp_pose_stamped = copy.deepcopy(
self.recognized_object.pose)
self.horizontal_center_grasp_pose_stamped = self.base_grasp_pose_stamped
self.temp_grasp_pose_stamped = self.base_grasp_pose_stamped
self.grasp_quality = 1.0
if recognized_object.dimensions[2] < self.high_res_threshold:
rospy.loginfo(
"Z axis of object ({} m) is shorter than threshold {}. \
create denser grasp poses.".format(
recognized_object.dimensions[2], self.high_res_threshold))
self.distance_vertical = self.distance_vertical_default / 2.0
self.padding_from_top_and_bottom = self.padding_from_top_and_bottom_default / 2.0
else:
rospy.loginfo(
"Z axis of object ({} m) is longer than threshold {}. \
create normal distance grasp poses.".format(
recognized_object.dimensions[2], self.high_res_threshold))
self.distance_vertical = self.distance_vertical_default
self.padding_from_top_and_bottom = self.padding_from_top_and_bottom_default
self.number_of_poses_vertical = int(
(recognized_object.dimensions[2] -
float(self.padding_from_top_and_bottom)) /
float(self.distance_vertical) / 2.0)
rospy.loginfo("Number of vertical poses " +
str(self.number_of_poses_vertical))
# Generate the central grasp pose. Based on this pose the other poses are generated.
# check if this is a top grasp (x axis faces in negative z direction of base_link) or a side grasp
# pose with x downward, z forward
pose_stamped_compare = PoseStamped()
pose_stamped_compare.header.frame_id = "base_link"
pose_stamped_compare.pose.position.x = 0.0
pose_stamped_compare.pose.position.y = 0.0
pose_stamped_compare.pose.position.z = 0.0
pose_stamped_compare.pose.orientation.w = 0.7071
pose_stamped_compare.pose.orientation.x = 0.0
pose_stamped_compare.pose.orientation.y = 0.7071
pose_stamped_compare.pose.orientation.z = 0.0
angle = self.angle_between_poses(self.temp_grasp_pose_stamped,
pose_stamped_compare)
if angle < pi / 4:
# do a pinch grasp
rospy.loginfo("This grasp is a top grasp. Do a pinch grasp")
self.x_offset = -recognized_object.dimensions[
0] / 2 + self.palm_to_finger_tips_distance
self.recognized_object.top_grasp = True
else:
rospy.loginfo("This grasp is a side grasp. Do a power grasp")
self.x_offset = recognized_object.dimensions[
0] / 2 + self.additional_offset_in_grasp_direction
self.recognized_object.top_grasp = False
self.shift_pose_along_the_grippers_axes(self.temp_grasp_pose_stamped,
[-self.x_offset, 0, 0])
self.create_grasp(self.temp_grasp_pose_stamped)
# vertical
for vertical_counter in range(0, self.number_of_poses_vertical + 1):
if self.recognized_object.rotational_symmetric:
self.generate_horizontal_grasps(True)
self.generate_vertical_grasp_upper_part(vertical_counter)
self.generate_horizontal_grasps(True)
self.generate_vertical_grasp_lower_part(vertical_counter)
else:
self.generate_horizontal_grasps(False)
self.generate_vertical_grasp_upper_part(vertical_counter)
self.generate_horizontal_grasps(False)
self.generate_vertical_grasp_lower_part(vertical_counter)
rospy.loginfo("Generated vertical pose " + str(vertical_counter))
rospy.loginfo("Number of generated grasps: " + str(len(self.grasps)))
for grasp in self.grasps:
# rospy.logdebug("Publishing grasp z position:" + str(grasp.grasp_pose.pose.position))
# rospy.loginfo("Grasp quality: %s" % (grasp.grasp_quality))
return self.grasps
def generate_vertical_grasp_upper_part(self, vertical_counter):
self.generate_vertical_grasp("upper", vertical_counter)
def generate_vertical_grasp_lower_part(self, vertical_counter):
self.generate_vertical_grasp("lower", vertical_counter)
def generate_vertical_grasp(self, part, vertical_counter):
if part == "upper":
distance_vertical = self.distance_vertical
elif part == "lower":
distance_vertical = -self.distance_vertical
else:
rospy.logerror('Part %s unkown. Only use "upper" or "lower"' %
(part))
self.horizontal_center_grasp_pose_stamped = copy.deepcopy(
self.base_grasp_pose_stamped)
self.temp_grasp_pose_stamped = self.horizontal_center_grasp_pose_stamped
marvin_manipulation.transformations_common.shift_pose_along_the_grippers_axes(
self.temp_grasp_pose_stamped,
[0, 0, float(distance_vertical) * float(vertical_counter)])
self.grasp_quality = 1.0 / (float(vertical_counter) + 0.001
) #avoid devided by zero
self.create_grasp(self.temp_grasp_pose_stamped)
def generate_horizontal_grasps(self, rotational_symmetric):
if rotational_symmetric:
rospy.loginfo('Rotational symmetric')
for horizontal_counter in range(
-self.number_of_poses_horizontal,
self.number_of_poses_horizontal + 1):
self.temp_grasp_pose_stamped = copy.deepcopy(
self.horizontal_center_grasp_pose_stamped)
self.temp_grasp_pose_stamped = self.rotate_pose_around_object_center(
self.temp_grasp_pose_stamped,
self.angle_horizontal * float(horizontal_counter),
self.x_offset)
self.create_grasp(self.temp_grasp_pose_stamped)
else:
rospy.loginfo('non symmetric')
#create grasp facing in negative x direction
self.temp_grasp_pose_stamped = copy.deepcopy(
self.horizontal_center_grasp_pose_stamped)
self.temp_grasp_pose_stamped = self.rotate_pose_around_object_center(
self.temp_grasp_pose_stamped, pi, self.x_offset)
self.create_grasp(self.temp_grasp_pose_stamped)
def rotate_pose_around_object_center(self, pose_stamped, angle,
distance_to_object_center):
self.shift_pose_along_the_grippers_axes(
pose_stamped, (distance_to_object_center, 0, 0))
self.rotate_pose_around_axis_by_angle(pose_stamped, angle, (0, 0, 1))
self.shift_pose_along_the_grippers_axes(
pose_stamped, (-distance_to_object_center, 0, 0))
return pose_stamped
def create_grasp(self, pose_stamped):
grasp = Grasp()
grasp.grasp_pose = copy.deepcopy(pose_stamped)
# pre grasp gripper configuration
grasp.pre_grasp_posture.header.frame_id = self.gripper_frame
grasp.pre_grasp_posture.header.stamp = rospy.Time.now()
grasp.pre_grasp_posture.joint_names = self.finger_joint_names
grasp.pre_grasp_posture.points.append(
JointTrajectoryPoint(positions=[
self.gripper_open_angle_radian, self.gripper_open_angle_radian
],
effort=[self.max_effort, self.max_effort]))
# approach
grasp.pre_grasp_approach.direction.header.frame_id = self.gripper_frame
grasp.pre_grasp_approach.direction.vector.x = 1.0
grasp.pre_grasp_approach.min_distance = 0.04
grasp.pre_grasp_approach.desired_distance = 0.15
# grasp gripper configuration
grasp.grasp_posture.header.frame_id = self.gripper_frame
grasp.grasp_posture.header.stamp = rospy.Time.now()
grasp.grasp_posture.joint_names = self.finger_joint_names
grasp.grasp_posture.points.append(
JointTrajectoryPoint(positions=[
self.gripper_closed_angle_radian,
self.gripper_closed_angle_radian
],
effort=[self.max_effort, self.max_effort]))
# TODO: add time_from_start here and for gripper open
# retreat
grasp.post_grasp_retreat.direction.header.frame_id = "base_link"
grasp.post_grasp_retreat.direction.vector.z = 1.0
grasp.post_grasp_retreat.min_distance = 0.03
grasp.post_grasp_retreat.desired_distance = 0.05
# objects allowed to touch while approaching
grasp.allowed_touch_objects = [self.recognized_object.name]
grasp.grasp_quality = self.grasp_quality
self.grasps.append(grasp)
def angle_between_poses(pose_stamped, pose_stamped_compare=None):
if not pose_stamped_compare:
#pose with x forward, z upward
pose_stamped_compare = PoseStamped()
pose_stamped_compare.header.frame_id = "base_link"
pose_stamped_compare.pose.position.x = 0.0
pose_stamped_compare.pose.position.y = 0.0
pose_stamped_compare.pose.position.z = 0.0
pose_stamped_compare.pose.orientation.w = 1.0
pose_stamped_compare.pose.orientation.x = 0.0
pose_stamped_compare.pose.orientation.y = 0.0
pose_stamped_compare.pose.orientation.z = 0.0
quaternion = [
pose_stamped.pose.orientation.x, pose_stamped.pose.orientation.y,
pose_stamped.pose.orientation.z, pose_stamped.pose.orientation.w
]
compare_quaternion = [
pose_stamped_compare.pose.orientation.x,
pose_stamped_compare.pose.orientation.y,
pose_stamped_compare.pose.orientation.z,
pose_stamped_compare.pose.orientation.w
]
inner_product = numpy.inner(quaternion, compare_quaternion)
angle_between_poses = math.degrees(math.acos(2 * inner_product**2 - 1))
print("Angle between poses: %s" % angle_between_poses)
return angle_between_poses
def shift_pose_along_the_grippers_axes(pose_stamped, shift_per_axis_array):
'''
Shifts given pose_stamped by the amounts given in shift_per_axis_array
:param PoseStamped pose_stamped:
:param () shift_per_axis_array: tupel of translations e.g. (0.5, 0, 0) to shift the pose 0.5m in x direction
'''
frame = PyKDL.Frame(
PyKDL.Rotation.Quaternion(pose_stamped.pose.orientation.x,
pose_stamped.pose.orientation.y,
pose_stamped.pose.orientation.z,
pose_stamped.pose.orientation.w),
PyKDL.Vector(pose_stamped.pose.position.x,
pose_stamped.pose.position.y,
pose_stamped.pose.position.z))
point = PyKDL.Vector(shift_per_axis_array[0], shift_per_axis_array[1],
shift_per_axis_array[2])
point = frame * point
pose_stamped.pose.position.x = point.x()
pose_stamped.pose.position.y = point.y()
pose_stamped.pose.position.z = point.z()
def place(self, recognized_object, pose_stamped):
result = self.arm.place(recognized_object.name, pose_stamped)
rospy.loginfo("Place result: " + str(result))
if result == True:
return result
return result
class RobotMotionObserver(object):
"""Observes motions of the robot.
The implementation is based on using the get_current_joint_values()) functionality of the RobotCommander().
:param group_name: Name of the planning group, default value is 'manipulator'
"""
_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC = 3.0
_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD = 0.01
_DEFAULT_SLEEP_INTERVAL_SEC = 0.01
_DEFAULT_GROUP_NAME = "manipulator"
def __init__(self, group_name=_DEFAULT_GROUP_NAME):
self._robot_commander = RobotCommander()
self._group_name = group_name
@staticmethod
def _detect_motion(joint_values_a,
joint_values_b,
tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD):
"""TRUE if a significant motion was detected, otherwise FALSE."""
return not numpy.allclose(
joint_values_a, joint_values_b, atol=tolerance)
def _get_current_joint_states(self):
"""Returns the current joint state values of the robot.
:raises RobotCurrentStateError if given planning group does not exist.
"""
try:
return self._robot_commander.get_group(
self._group_name).get_current_joint_values()
except MoveItCommanderException as e:
rospy.logerr(e.message)
raise RobotCurrentStateError(e.message)
def is_robot_moving(
self,
sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC,
move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD):
"""TRUE if the robot is currently moving, otherwise FLASE."""
start_position = self._get_current_joint_states()
rospy.sleep(sleep_interval)
return RobotMotionObserver._detect_motion(
start_position, self._get_current_joint_states(), move_tolerance)
def wait_motion_start(
self,
wait_time_out=_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC,
move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC):
"""TRUE if the motion started in the given time interval, otherwise FALSE."""
old_joint_values = self._get_current_joint_states()
rospy.loginfo("Start joint values: " + str(old_joint_values))
motion_started = False
start_time = rospy.get_time()
rospy.loginfo("Wait until motion starts...")
while True:
rospy.sleep(sleep_interval)
curr_joint_values = self._get_current_joint_states()
if RobotMotionObserver._detect_motion(curr_joint_values,
old_joint_values,
move_tolerance):
motion_started = True
rospy.loginfo("Changed joint values detected: " +
str(curr_joint_values))
rospy.loginfo("Motion started.")
break
if (rospy.get_time() - start_time > wait_time_out):
rospy.loginfo("Reached timeout when waiting for motion start.")
break
return motion_started
def wait_motion_stop(
self,
wait_time_out=_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC,
move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC):
"""TRUE if the motion stopped in the given time interval, otherwise FALSE."""
motion_stopped = False
start_time = rospy.get_time()
rospy.loginfo("Wait until motion stops...")
while True:
if not self.is_robot_moving(sleep_interval, move_tolerance):
motion_stopped = True
rospy.loginfo("Motion stopped.")
break
if (rospy.get_time() - start_time > wait_time_out):
rospy.loginfo("Reached timeout when waiting for motion stop.")
break
return motion_stopped
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------------------"
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, 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)
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)
p.pose.position.x = 0.5
p.pose.position.y = 0
p.pose.position.z = 0.5
planning_scene_interface.add_box("object", p, (0.02, 0.02, 0.2))
if __name__=='__main__':
# Initialise moveit and ROS node
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
# Get handles to the planning scene and robot
scene = PlanningSceneInterface()
robot = RobotCommander()
# Select planning group
group = robot.get_group('panda_arm')
# Set a liberal planner timeout
group.set_planning_time(seconds=45.0)
# IMPORTANT: you must call sleep after the previous line
# to ensure the planning scene object is initialised before
# using it.
rospy.sleep(2)
# Setup scene for the pick and place example
addCollisionObjects(scene)
rospy.sleep(1)
# Plan and execute the pick operation
class TestFacilitySensors(object):
"""Abstraction layer for easy usage of the PILZ test facility sensors."""
_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC = 3.0
_DEFAULT_WAIT_TIME_IS_ROBOT_MOVING_SEC = 1.0
_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD = 0.01
_DEFAULT_SLEEP_INTERVAL_SEC = 0.01
_DEFAULT_GROUP_NAME = "manipulator"
def __init__(self, group_name=_DEFAULT_GROUP_NAME):
self._robot_commander = RobotCommander()
self._group_name = group_name
def set_default_motion_detection_tolerance(self, motion_tolerance_rad):
self._DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD = motion_tolerance_rad
def _get_current_joint_states(self):
"""Returns the current joint state values of the robot.
:raises RobotCurrentStateError if given planning group does not exist.
"""
return self._robot_commander.get_group(
self._group_name).get_current_joint_values()
def is_robot_at_position(
self,
expected_joint_values,
tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD):
"""True if the robot stands at the specified position, otherwise False."""
return _are_joint_values_equal(self._get_current_joint_states(),
expected_joint_values, tolerance)
def is_robot_moving(self,
wait_time_out=_DEFAULT_WAIT_TIME_IS_ROBOT_MOVING_SEC):
"""True if the robot is moving, otherwise false."""
return self.wait_for_robot_motion(wait_time_out=wait_time_out)
def _wait_till(self, motion_expected, wait_time_out, move_tolerance,
sleep_interval):
"""Waits till the robot has started moving or until the robot has stopped, depending on the given parameters.
"""
old_joint_values = self._get_current_joint_states()
rospy.loginfo("Start joint values: " + str(old_joint_values))
start_time = rospy.get_time()
rospy.loginfo("Start observing robot motion...")
while not rospy.is_shutdown():
rospy.sleep(sleep_interval)
curr_joint_values = self._get_current_joint_states()
motion_detected = not _are_joint_values_equal(
curr_joint_values, old_joint_values, move_tolerance)
if motion_expected and motion_detected:
rospy.loginfo("Motion detected at pose: " +
str(curr_joint_values))
return True
if not motion_expected and not motion_detected:
rospy.loginfo("Robot stands still at pose: " +
str(curr_joint_values))
return True
if rospy.get_time() - start_time > wait_time_out:
rospy.loginfo("Timeout reached")
return False
def wait_for_robot_motion(
self,
wait_time_out=_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC,
move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC):
"""Waits until a robot motion is detected."""
return self._wait_till(motion_expected=True,
wait_time_out=wait_time_out,
move_tolerance=move_tolerance,
sleep_interval=sleep_interval)
def wait_till_robot_stopped(
self,
wait_time_out=_DEFAULT_WAIT_TIME_FOR_MOTION_DETECTION_SEC,
move_tolerance=_DEFAULT_TOLERANCE_FOR_MOTION_DETECTION_RAD,
sleep_interval=_DEFAULT_SLEEP_INTERVAL_SEC):
"""Waits till the robot stops it's motion."""
return self._wait_till(motion_expected=False,
wait_time_out=wait_time_out,
move_tolerance=move_tolerance,
sleep_interval=sleep_interval)
def commander():
global solution
# Makes a ROS node #
rospy.init_node('rubik_commander', anonymous=True)
# Gets a handle on the robot that MoveIt is controlling
robot = RobotCommander()
rospy.sleep(1)
rospy.Subscriber('cube_steps', StringArray, callback)
# Picks a group called "manipulator" to work with #
arm = robot.get_group("manipulator")
arm.set_max_velocity_scaling_factor(1.0)
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.005)
rate = rospy.Rate(2)
while not rospy.is_shutdown():
if len(solution) > 0:
for move in solution:
print("### " + str(move) + " ###")
##### PICK #####
go_home(arm)
if "F" in move:
front_to_right(arm, robot)
go_home(arm)
pick_rubik(arm, robot, "right")
if "B" in move:
pick_rubik(arm, robot, "back")
if "L" in move:
pick_rubik(arm, robot, "left")
if "R" in move:
pick_rubik(arm, robot, "right")
if "U" in move:
expose_up_down(arm, robot)
go_home(arm)
pick_rubik(arm, robot, "right")
if "D" in move:
expose_up_down(arm, robot)
go_home(arm)
pick_rubik(arm, robot, "left")
go_home(arm)
##### TURN #####
if "'" in move:
turn_rubik(arm, robot, "counter")
elif "2" in move:
turn_rubik(arm, robot, "180")
else:
turn_rubik(arm, robot, "clockwise")
go_home(arm)
##### PLACE
if "F" in move:
place_rubik(arm, robot, "right")
go_home(arm)
right_to_front(arm, robot)
if "B" in move:
place_rubik(arm, robot, "back")
if "L" in move:
place_rubik(arm, robot, "left")
if "R" in move:
place_rubik(arm, robot, "right")
if "U" in move:
place_rubik(arm, robot, "right")
go_home(arm)
reset_up_down(arm, robot)
if "D" in move:
place_rubik(arm, robot, "left")
go_home(arm)
reset_up_down(arm, robot)
go_home(arm)
solution = []
rate.sleep()
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()
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)
#
# self._pose_table = self._scene.get_object_poses(self._table_object_name)
# self._pose_coke_can = self._scene.get_object_poses(self._grasp_object_name)
# self.setup()
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
rospy.sleep(1.0)
self._scene.remove_world_object(self._grasp_object_name)
rospy.sleep(1.0)
# # Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
rospy.sleep(2.0)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.sleep(2.0)
# 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))
# 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 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)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
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_attached_object()
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
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)
def _add_objects(self):
"""
Here add all the objects that you want to add to the _scene
"""
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
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()
