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 __init__(self, name):
# stuff for grasp planning
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_objects = RecognizedObjectArray()
#rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
#pickup_ac.wait_for_server() # needed?
rospy.loginfo("Connecting to grasp generator AS")
self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
#grasps_ac.wait_for_server() # needed?
#planning scene for motion planning
self.scene = PlanningSceneInterface()
# blocking action server
self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
self.feedback = GraspObjectFeedback()
self.result = GraspObjectResult()
self.current_goal = None
self.grasp_obj_as.start()
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def __init__(self):
self.node_name = "PickAndPlaceServer"
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
# Get the object size
self.object_height = 0.1
self.object_width = 0.05
self.object_depth = 0.05
self.pick_pose = rospy.get_param('~pickup_marker_pose')
self.place_pose = rospy.get_param('~place_marker_pose')
rospy.loginfo("%s: Waiting for pickup action server...", self.node_name)
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
connected = self.pickup_ac.wait_for_server(rospy.Duration(3000))
if not connected:
rospy.logerr("%s: Could not connect to pickup action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to pickup action server", self.node_name)
rospy.loginfo("%s: Waiting for place action server...", self.node_name)
self.place_ac = SimpleActionClient('/place', PlaceAction)
if not self.place_ac.wait_for_server(rospy.Duration(3000)):
rospy.logerr("%s: Could not connect to place action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to place action server", self.node_name)
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
else:
rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction,
execute_cb=self.pick_cb, auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction,
execute_cb=self.place_cb, auto_start=False)
self.place_as.start()
def __init__(self, name):
# stuff for grasp planning
rospy.loginfo("Getting a TransformListener...")
self.tf_listener = tf.TransformListener()
rospy.loginfo("Getting a TransformBroadcaster...")
self.tf_broadcaster = tf.TransformBroadcaster()
rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_clusters = None
rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)
if DEBUG_MODE:
self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)
rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
self.grasps_ac.wait_for_server()
rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
self.depth_service.wait_for_service()
rospy.loginfo("Getting a PlanningSceneInterface instance...")
self.scene = PlanningSceneInterface()
# blocking action server
rospy.loginfo("Creating Action Server '" + name + "'...")
self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
self.as_feedback = ObjectManipulationFeedback()
self.as_result = ObjectManipulationActionResult()
self.current_goal = None
# Take care of left and right arm grasped stuff
self.right_hand_object = None
self.left_hand_object = None
self.current_side = 'right'
rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
self.grasp_obj_as.start()
def __init__(self):
rospy.init_node('moveit_web',disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states',JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text':'ready to plan','ready':True}
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('scene_generator')
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
def __init__(self):
smach.State.__init__(self,
outcomes=['succeeded','failed'],
input_keys=[],
output_keys=['new_box'])
# initialize tf listener
self.listener = tf.TransformListener()
### Create a handle for the Move Group Commander
self.mgc = MoveGroupCommander("manipulator")
### Create a handle for the Planning Scene Interface
self.psi = PlanningSceneInterface()
#
### initialize service for gripper on universal arm
self.io_srv = rospy.ServiceProxy('set_io', SetIO)
self.eef_step = 0.01
self.jump_threshold = 2
rospy.logwarn("Initializing Grasp")
rospy.sleep(1)
def main():
rospy.init_node('moveit_py_place', anonymous=True)
#right_arm.set_planner_id("KPIECEkConfigDefault");
scene = PlanningSceneInterface()
robot = RobotCommander()
#group = MoveGroupCommander("head")
right_arm = MoveGroupCommander("right_arm")
#right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.logwarn("cleaning world")
GRIPPER_FRAME = 'gripper_bracket_f2'
#scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.67
p.pose.position.y = -0.
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part")
n_attempts += 1
print "Attempts pickup: ", n_attempts
rospy.sleep(0.2)
#robot.right_arm.pick("part")
#right_arm.go()
rospy.sleep(5)
# Author: Ioan Sucan
import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
import tf
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("arm")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Robot Links for arm:"
print robot.get_link_names("arm")
print "============ Robot Links for gripper:"
print robot.get_link_names("gripper")
print right_arm.get_end_effector_link()
print "============ Printing robot state"
#print robot.get_current_state()
print "============"
rospy.sleep(1)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 50
# Set a limit on the number of place attempts
max_place_attempts = 50
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('up')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.45
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 1.0, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = -0.45
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = -0.41
box1_pose.pose.position.y = -0.3
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = -0.39
box2_pose.pose.position.y = 0.3
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
arm.set_support_surface_name(table_id)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = -0.42
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.5
place_pose.pose.position.y = 0.5
place_pose.pose.position.z = 0
place_pose.pose.orientation.w = 1.0
# Set the start state to the current state
arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = arm.plan()
# Execute the planned trajectory
arm.execute(traj)
# Close the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
# Pause for a second
rospy.sleep(2)
# Set the place pose of the end effector to the stored pose
arm.set_pose_target(place_pose, end_effector_link)
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('resting')
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class Planner(object):
move_group = None
goals = None
jspub = None
namespace = None
# These will eventually go to model objects
robot_data = {
'group_name': 'right_arm_and_torso',
'eef_link': 'r_wrist_joint_link'
}
# Current state of the 'session' (right now, only one)
current_scene = None
status = None
link_poses = None
def __init__(self):
rospy.init_node('moveit_web',disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states',JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text':'ready to plan','ready':True}
def get_scene(self):
return self.current_scene
def set_scene(self, scene):
self.current_scene = scene
psw = PlanningSceneWorld()
for co_json in scene['objects']:
# TODO: Fix orientation by using proper quaternions on the client
pose = self._make_pose(co_json['pose'])
# TODO: Decide what to do with STL vs. Collada. The client has a Collada
# loader but the PlanningSceneInterface can only deal with STL.
# TODO: Proper mapping between filenames and URLs
# filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
co = self.ps.make_mesh(co_json['name'], pose, filename)
psw.collision_objects.append(co)
self.psw_pub.publish(psw)
def get_link_poses(self):
if self.link_poses is None:
self.link_poses = self._move_group.get_link_poses_compressed()
return self.link_poses
# Create link back to socket.io namespace to allow emitting information
def set_socket(self, namespace):
self.namespace = namespace
def emit(self, event, data=None):
if self.namespace:
self.namespace.emit(event, data)
def emit_new_goal(self, pose):
self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])
def set_random_goal(self):
goal_pose = self.move_group.get_random_pose()
# goal_pose = self.move_group.get_random_pose('base_footprint')
self.emit_new_goal(goal_pose)
def _make_pose(self, json_pose):
pose = PoseStamped()
pose.header.frame_id = "odom_combined"
pp = json_pose['position']
pose.pose.position.x = pp['x']
pose.pose.position.y = pp['y']
pose.pose.position.z = pp['z']
# TODO: Orientation is not working. See about
# properly using Quaternions everywhere
pp = json_pose['orientation']
pose.pose.orientation.x = pp['x']
pose.pose.orientation.y = pp['y']
pose.pose.orientation.z = pp['z']
pose.pose.orientation.w = pp['w']
return pose
def plan_to_poses(self, poses):
goal_pose = self._make_pose(poses[0])
self.move_group.set_pose_target(goal_pose)
# self.move_group.set_pose_target(goal_pose,'base_footprint')
self.emit('status',{'text':'Starting to plan'})
trajectory = self.move_group.plan()
if trajectory is None or len(trajectory.joint_trajectory.joint_names) == 0:
self.status = {'reachable':False,'text':'Ready to plan','ready':True}
self.emit('status', self.status)
else:
self.status = {'reachable':True,'text':'Rendering trajectory'}
self.emit('status', self.status)
self.publish_trajectory(trajectory)
def publish_goal_position(self, trajectory):
self.publish_position(self, trajectory, -1)
def publish_position(self, trajectory, step):
jsmsg = JointState()
jsmsg.name = trajectory.joint_trajectory.joint_names
jsmsg.position = trajectory.joint_trajectory.points[step].positions
self.jspub.publish(jsmsg)
def publish_trajectory(self, trajectory):
cur_time = 0.0
acceleration = 4.0
for i in range(len(trajectory.joint_trajectory.points)):
point = trajectory.joint_trajectory.points[i]
gevent.sleep((point.time_from_start - cur_time)/acceleration)
cur_time = point.time_from_start
# self.publish_position(trajectory, i)
# TODO: Only say "True" to update state on the last step of the trajectory
new_poses = self._move_group.update_robot_state(trajectory.joint_trajectory.joint_names,
trajectory.joint_trajectory.points[i].positions, True)
self.link_poses = new_poses
self.emit('link_poses', new_poses)
self.status = {'text':'Ready to plan','ready':True}
self.emit('status', self.status)
class CokeCanPickAndPlace:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.6)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.01, 0.01, 0.009))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' %
self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -1
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' %
self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' %
self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_obstacles_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.65
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.35
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.3
box1_pose.pose.position.y = 0
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.3
box2_pose.pose.position.y = 0.25
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.22
target_pose.pose.position.y = 0.14
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the target pose for the arm
arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class scene_generator:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('scene_generator')
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
################################################################# FUNCTIONS #################################################################################
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
next_call = time.time()
while True:
next_call = next_call+1
target_id = 'target'
self.taid = self.pwh.name.index('custom_1')
table_id = 'table'
self.tid = self.pwh.name.index('table')
obstacle1_id = 'obstacle1'
self.o1id = self.pwh.name.index('wood_cube_5cm')
obstacle2_id = 'obstacle2'
self.o2id = self.pwh.name.index('custom_2')
# Set the sizes [l, w, h]
table_size = [1.5, 0.8, 0.03]
target_size = [0.05, 0.05, 0.15]
obstacle1_size = [0.05, 0.05, 0.05]
obstacle2_size = [0.05, 0.05, 0.10]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
if self.obj_att is None:
# Add the target object to the scene
self.scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
self.scene.add_box(table_id, table_pose, table_size)
obstacle1_pose = PoseStamped()
obstacle1_pose.header.frame_id = REFERENCE_FRAME
obstacle1_pose.pose = self.pwh.pose[self.o1id]
obstacle1_pose.pose.position.z += 0.025
# Add the target object to the scene
self.scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
obstacle2_pose = PoseStamped()
obstacle2_pose.header.frame_id = REFERENCE_FRAME
obstacle2_pose.pose = self.pwh.pose[self.o2id]
# Add the target object to the scene
self.scene.add_box(obstacle2_id, obstacle2_pose, obstacle2_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
time.sleep(next_call - time.time())
#threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('moveit_demo')
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
print "===== It is OK ===="
rospy.sleep(3)
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=1)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
left_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
left_reference_frame = left_arm.get_planning_frame()
# Set the left arm reference frame
left_arm.set_pose_reference_frame('base')
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 10
# Give the scene a chance to catch up
rospy.sleep(2)
object1_id = 'object1'
table_id = 'table'
target_id = 'target'
tool_id = 'tool'
#obstacle1_id = 'obstacle1'
# Remove leftover objects from a previous run
scene.remove_world_object(object1_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object('base', target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.0
object1_size = [0.088, 0.04, 0.02]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
#obstacle1_size = [0.3, 0.05, 0.45]
# Add a table top and two boxes to the scene
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = left_reference_frame
#obstacle1_pose.pose.position.x = 0.96
#obstacle1_pose.pose.position.y = 0.24
#obstacle1_pose.pose.position.z = 0.04
#obstacle1_pose.pose.orientation.w = 1.0
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
#self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)
object1_pose = PoseStamped()
object1_pose.header.frame_id = left_reference_frame
object1_pose.pose.position.x = 0.80
object1_pose.pose.position.y = 0.04
object1_pose.pose.position.z = table_ground + table_size[
2] + object1_size[2] / 2.0
object1_pose.pose.orientation.w = 1.0
scene.add_box(object1_id, object1_pose, object1_size)
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = left_reference_frame
table_pose.pose.position.x = 1
table_pose.pose.position.y = 0.7
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = left_reference_frame
target_pose.pose.position.x = 0.58
target_pose.pose.position.y = 0.1878
target_pose.pose.position.z = .1116
target_pose.pose.orientation.x = 0.1325
target_pose.pose.orientation.y = 0.9908
target_pose.pose.orientation.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 0.0254
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(object1_id, 0.8, 0, 0, 1.0)
self.setColor(table_id, 0.8, 0, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
left_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = left_reference_frame
place_pose.pose.position.x = 0.18
place_pose.pose.position.y = -0.18
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
#grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = left_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = left_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
# Open the gripper to the neutral position
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_fk_demo', anonymous=True)
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('xarm6')
# 设置机械臂运动的允许误差值
arm.set_goal_joint_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 移除场景中之前运行残留的物体
scene.remove_world_object('eaibot_n1')
# 设置box的高度
eaibot_n1_height = 0
# 设置box的三维尺寸
eaibot_n1_size = [0.65, 0.7, 0.01]
# 将box加入场景当中
eaibot_n1_pose = PoseStamped()
eaibot_n1_pose.header.frame_id = 'link_base'
eaibot_n1_pose.pose.position.x = 0.0
eaibot_n1_pose.pose.position.y = 0.0
eaibot_n1_pose.pose.position.z = eaibot_n1_height + eaibot_n1_size[
2] / 2.0
eaibot_n1_pose.pose.orientation.w = 1.0
scene.add_box('eaibot_n1', eaibot_n1_pose, eaibot_n1_size)
rospy.sleep(2)
# 移除场景中之前运行残留的物体
scene.remove_world_object('ground')
# 设置box的高度
ground_height = -0.3
# 设置box的三维尺寸
ground_size = [2, 2, 0.01]
# 将box加入场景当中
ground_pose = PoseStamped()
ground_pose.header.frame_id = 'link_base'
ground_pose.pose.position.x = 0.0
ground_pose.pose.position.y = 0.0
ground_pose.pose.position.z = ground_height + ground_size[2] / 2.0
ground_pose.pose.orientation.w = 1.0
scene.add_box('ground', ground_pose, ground_size)
rospy.sleep(2)
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [
-0.001745, 1.394518, -1.9059, -0.013963, 0.513127, 0.005236
]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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_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)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.sleep(1.0)
##############################################################################################################################
##############################################################################################################################
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.20
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))
###different reffere
#reference frame is base_link
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
# 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('pickup')
self._arm.go(wait=True)
print("Pickup pose")
# 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')
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(1.0)
rospy.loginfo('Place successfully')
class TestPlanners(object):
def __init__(self, group_id, planner):
rospy.init_node('moveit_test_planners', anonymous=True)
self.pass_list = []
self.fail_list = []
self.planner = planner
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(group_id)
rospy.sleep(1)
self.group.set_planner_id(self.planner)
self.test_trajectories_empty_environment()
self.test_waypoints()
self.test_trajectories_with_walls_and_ground()
for pass_test in self.pass_list:
print(pass_test)
for fail_test in self.fail_list:
print(fail_test)
def _add_walls_and_ground(self):
# publish a scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below ground (to prevent collision with
# the robot itself)
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.1))
p.pose.position.x = 0.4
p.pose.position.y = 0.85
p.pose.position.z = 0.4
p.pose.orientation.x = 0.5
p.pose.orientation.y = -0.5
p.pose.orientation.z = 0.5
p.pose.orientation.w = 0.5
self.scene.add_box("wall_front", p, (0.8, 2, 0.01))
p.pose.position.x = 1.33
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707388
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.706825
self.scene.add_box("wall_right", p, (0.8, 2, 0.01))
p.pose.position.x = -0.5
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707107
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.707107
self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
# rospy.sleep(1)
def _check_plan(self, plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
def _plan_joints(self, joints):
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
return self._check_plan(plan)
def test_trajectories_rotating_each_joint(self):
# test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
test_joint_values = [numpy.pi / 2.0]
joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
# Joint 4th is colliding with the hand
# for joint in range(6):
for joint in [0, 1, 2, 3, 5]:
for value in test_joint_values:
joints[joint] = value
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
def test_trajectories_empty_environment(self):
# Up - Does not work with sbpl but it does with ompl
joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# All joints up
joints = [
-1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Down
joints = [
-0.000348431194526, 0.397651011661, 0.0766181197394,
-0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# left
joints = [
0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
0.146075718452, 0.00420656698366, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Front
joints = [
1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
-1.42700242769, 3.1415450794, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Behind
joints = [
1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
0.694689321451, -0.390769365032, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Should fail because it is in self-collision
joints = [
-0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
1.04235601797, -1.69730925867, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
def test_waypoints(self):
# Start planning in a given joint position
joints = [
-0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
0.344227622185, -3.03250264451, 0.0, 0.0
]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:8] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
waypoints = []
initial_pose = self.group.get_current_pose().pose
initial_pose.position.x = -0.301185959729
initial_pose.position.y = 0.517069787724
initial_pose.position.z = 1.20681710541
initial_pose.orientation.x = 0.0207499700474
initial_pose.orientation.y = -0.723943002716
initial_pose.orientation.z = -0.689528413407
initial_pose.orientation.w = 0.00515118111221
# start with a specific position
waypoints.append(initial_pose)
# first move it down
wpose = geometry_msgs.msg.Pose()
wpose.orientation = waypoints[0].orientation
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z - 0.20
waypoints.append(copy.deepcopy(wpose))
# second front
wpose.position.y += 0.20
waypoints.append(copy.deepcopy(wpose))
# third side
wpose.position.x -= 0.20
waypoints.append(copy.deepcopy(wpose))
# fourth return to initial pose
wpose = waypoints[0]
waypoints.append(copy.deepcopy(wpose))
(plan3,
fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
if not self._check_plan(plan3) and fraction > 0.8:
self.fail_list.append("Failed: test_waypoints, " + self.planner)
else:
self.pass_list.append("Passed: test_waypoints, " + self.planner)
def test_trajectories_with_walls_and_ground(self):
self._add_walls_and_ground()
# Should fail to plan: Goal is in collision with the wall_front
joints = [
0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
0.302143499777, 0.00130280337897, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Should fail to plan: Goal is in collision with the ground
joints = [
3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
0.000133883149666, -0.594498939239, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Goal close to right corner
joints = [
0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
-1.3516255551, 2.8225061435, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed, test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
self.scene.remove_world_object("ground")
self.scene.remove_world_object("wall_left")
self.scene.remove_world_object("wall_right")
self.scene.remove_world_object("wall_front")
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
left_arm = MoveGroupCommander('left_arm')
left_gripper = MoveGroupCommander('left_gripper')
# Get the name of the end-effector link
left_eef = left_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
left_arm.set_goal_position_tolerance(0.01)
left_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
left_arm.allow_replanning(True)
left_reference_frame = left_arm.get_planning_frame()
# Allow 5 seconds per planning attempt
left_arm.set_planning_time(5)
object1_id = 'object1'
obstacle1_id = 'obstacle1'
# Remove leftover objects from a previous run
scene.remove_world_object(object1_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
left_arm.set_named_target('left_arm_zero')
left_arm.go()
rospy.sleep(1)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
object1_size = [0.088, 0.04, 0.02]
# Add a table top and two boxes to the scene
obstacle1_size = [0.3, 0.05, 0.45]
# Add a table top and two boxes to the scene
obstacle1_pose = PoseStamped()
obstacle1_pose.header.frame_id = left_reference_frame
obstacle1_pose.pose.position.x = 0.96
obstacle1_pose.pose.position.y = 0.5
obstacle1_pose.pose.position.z = 0.04
obstacle1_pose.pose.orientation.w = 1.0
scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0)
object1_pose = PoseStamped()
object1_pose.header.frame_id = left_reference_frame
object1_pose.pose.position.x = 0.80
object1_pose.pose.position.y = 0.3
object1_pose.pose.position.z = 0
object1_pose.pose.orientation.w = 1.0
scene.add_box(object1_id, object1_pose, object1_size)
self.setColor(object1_id, 0.8, 0, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the start state to the current state
left_arm.set_start_state_to_current_state()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = left_reference_frame
target_pose.pose.position.x = 0.58
target_pose.pose.position.y = 0.1878
target_pose.pose.position.z = .1116
target_pose.pose.orientation.x = 0.1325
target_pose.pose.orientation.y = 0.9908
target_pose.pose.orientation.z = 0.0095
target_pose.pose.orientation.w = 0.0254
# Set the target pose for the arm
left_arm.set_pose_target(target_pose, left_eef)
# Move the arm to the target pose (if possible)
left_arm.go()
# Pause for a moment...
rospy.sleep(2)
left_gripper.set_joint_value_target(GRIPPER_CLOSE)
left_gripper.go()
rospy.sleep(1)
scene.attach_box(left_eef, object1_id, object1_pose, object1_size)
# Cartesian Paths
waypoints1 = []
start_pose = left_arm.get_current_pose(left_eef).pose
wpose = deepcopy(start_pose)
waypoints1.append(deepcopy(wpose))
wpose.position.x -= 0.05
wpose.position.y += 0.105
wpose.position.z += 0.07
waypoints1.append(deepcopy(wpose))
wpose.position.x -= 0.05
wpose.position.y += 0.105
wpose.position.z -= 0.07
waypoints1.append(deepcopy(wpose))
(cartesian_plan, fraction) = left_arm.compute_cartesian_path(
waypoints1, # waypoint poses
0.01, # eef_step 1cm
0.0, # jump_threshold
True) # avoid_collisions
left_arm.execute(cartesian_plan)
rospy.sleep(2)
left_gripper.set_joint_value_target(GRIPPER_OPEN)
left_gripper.go()
rospy.sleep(1)
scene.remove_attached_object(left_eef, object1_id)
# Exit MoveIt cleanly
waypoints2 = []
start_pose = left_arm.get_current_pose(left_eef).pose
wpose = deepcopy(start_pose)
waypoints2.append(deepcopy(wpose))
wpose.position.z += 0.07
waypoints2.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.y -= 0.21
waypoints2.append(deepcopy(wpose))
wpose.position.z -= 0.07
waypoints2.append(deepcopy(wpose))
(cartesian_plan, fraction) = left_arm.compute_cartesian_path(
waypoints2, # waypoint poses
0.01, # eef_step 1cm
0.0, # jump_threshold
True) # avoid_collisions
left_arm.execute(cartesian_plan)
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'coke_can')
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.3)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_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 + 0.05
self._pose_place.position.y = self._pose_coke_can.position.y
self._pose_place.position.z = self._pose_coke_can.position.z - 0.1
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
rospy.loginfo('x = %f, y = %f, z = %f', self._pose_place.position.x,
self._pose_place.position.y, self._pose_place.position.z)
# 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')
# a = robot.right_arm
# a.set_start_state(RobotState())
# r = a.get_random_joint_values()
# print "Planning to random joint position: "
# print r
# p = a.plan(r)
# print "Solution:"
# print p
#
# roscpp_shutdown()
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)
# clean the scene
#scene.remove_world_object("pole")
#scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# p.pose.position.x = 0.7
# p.pose.position.y = -0.4
# p.pose.position.z = 1.15
p.pose.orientation.w = 1.0
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(2)
# 设置桌面的高度
#table_ground = 0.25
table_ground = 0.10
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
#table_pose.pose.position.x = 0.26
table_pose.pose.position.x = 0.30
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
#box1_pose.pose.position.x = 0.21
box1_pose.pose.position.x = 0.30
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.30
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置机械臂的运动目标位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.30
target_pose.pose.position.y = 0.10
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.05
target_pose.pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# 设置机械臂的运动目标位置，进行避障规划
target_pose2 = PoseStamped()
target_pose2.header.frame_id = reference_frame
target_pose2.pose.position.x = 0.30
target_pose2.pose.position.y = -0.10
target_pose2.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.05
target_pose2.pose.orientation.w = 1.0
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose2, end_effector_link)
arm.go()
rospy.sleep(2)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
## Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Prepare Gripper and open it
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
self.ac.send_goal(g_open)
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
#print target_pose
self.object_frames_pub.publish(target_pose)
rospy.sleep(2)
print "==================== Generating Transformations ==========================="
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
# # Initialize the grasp object
# g = Grasp()
# grasps = []
# # Set the first grasp pose to the input pose
# g.grasp_pose = pre_grasping
# g.allowed_touch_objects = [target_id]
# grasps.append(deepcopy(g))
# right_arm.pick(target_id, grasps)
#Change the frame_id for the planning to take place!
#target_pose.header.frame_id = 'gazebo_world'
self.p_pub.publish(pre_grasping)
right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME)
plan = right_arm.plan()
rospy.sleep(2)
right_arm.go(wait=True)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class WorldManager:
def __init__(self, server):
self.server = server
moveit_commander.roscpp_initialize(sys.argv)
self.planning_scene_topic = rospy.get_param("planning_scene_topic")
self.run_recognition_topic = rospy.get_param("run_recognition_topic")
self.detected_model_frame_id = rospy.get_param(
"detected_model_frame_id")
self.scene = PlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
self.model_manager = ModelRecManager()
self.planning_scene_service_proxy = rospy.ServiceProxy(
self.planning_scene_topic, moveit_msgs.srv.GetPlanningScene)
self._run_recognition_as = actionlib.SimpleActionServer(
self.run_recognition_topic,
graspit_msgs.msg.RunObjectRecognitionAction,
execute_cb=self._run_recognition_as_cb,
auto_start=False)
self._run_recognition_as.start()
rospy.loginfo("World Manager Node is Up and Running")
def _run_recognition_as_cb(self, goal):
print("_run_recognition_as_cb")
print("about to remove_all_objects_from_planner()")
self.remove_all_objects_from_planner()
print("finished remove_all_objects_from_planner()")
self.model_manager.refresh()
print("about to add_all_objects_to_planner()")
self.add_all_objects_to_planner()
print("finished add_all_objects_to_planner()")
_result = graspit_msgs.msg.RunObjectRecognitionResult()
print("graspit_msgs.msg.RunObjectRecognitionResult()")
for model in self.model_manager.model_list:
object_info = graspit_msgs.msg.ObjectInfo(model.object_name,
model.model_name,
model.get_world_pose())
_result.object_info.append(object_info)
position = model.get_world_pose().position
position = Point(position.x, position.y, position.z)
make6DofMarker(
fixed=False,
frame=model.model_name,
interaction_mode=InteractiveMarkerControl.MOVE_ROTATE_3D,
position=position,
show_6dof=True)
print("finished for loop")
server.applyChanges()
self._run_recognition_as.set_succeeded(_result)
return []
def get_body_names_from_planner(self):
rospy.wait_for_service(self.planning_scene_topic, 5)
components = PlanningSceneComponents(
PlanningSceneComponents.WORLD_OBJECT_NAMES +
PlanningSceneComponents.TRANSFORMS)
ps_request = moveit_msgs.srv.GetPlanningSceneRequest(
components=components)
ps_response = self.planning_scene_service_proxy.call(ps_request)
body_names = [
co.id for co in ps_response.scene.world.collision_objects
]
return body_names
def remove_all_objects_from_planner(self):
body_names = self.get_body_names_from_planner()
while len(body_names) > 0:
print(
"removing bodies from the planner, this can potentially take several tries"
)
for body_name in body_names:
self.scene.remove_world_object(body_name)
body_names = self.get_body_names_from_planner()
def add_all_objects_to_planner(self):
self.add_obstacles()
for model in self.model_manager.model_list:
model_name = model.model_name.strip('/')
print "Adding " + str(model_name) + "To Moveit"
filename = file_name_dict[model_name]
if os.path.isfile(filename):
stamped_model_pose = geometry_msgs.msg.PoseStamped()
stamped_model_pose.header.frame_id = self.detected_model_frame_id
stamped_model_pose.pose = model.get_world_pose()
self.scene.add_mesh(model.object_name, stamped_model_pose,
filename)
else:
rospy.logwarn('File doesn\'t exist - object %s, filename %s' %
(model.object_name, filename))
def add_walls(self):
walls = rospy.get_param('/walls')
for wall_params in walls:
rospy.loginfo("Adding wall " + str(wall_params))
self.add_wall(wall_params)
return
def add_wall(self, wall_params):
name = wall_params["name"]
x_thickness = wall_params["x_thickness"]
y_thickness = wall_params["y_thickness"]
z_thickness = wall_params["z_thickness"]
x = wall_params["x"]
y = wall_params["y"]
z = wall_params["z"]
qx = wall_params["qx"]
qy = wall_params["qy"]
qz = wall_params["qz"]
qw = wall_params["qw"]
frame_id = wall_params["frame_id"]
back_wall_pose = geometry_msgs.msg.PoseStamped()
back_wall_pose.header.frame_id = '/' + frame_id
wall_dimensions = [x_thickness, y_thickness, z_thickness]
back_wall_pose.pose.position = geometry_msgs.msg.Point(**{
'x': x,
'y': y,
'z': z
})
back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{
'x': qx,
'y': qy,
'z': qz,
'w': qw
})
self.scene.add_box(name, back_wall_pose, wall_dimensions)
return
def add_obstacles(self):
self.add_walls()
class GraspObjectServer:
def __init__(self, name):
# stuff for grasp planning
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_objects = RecognizedObjectArray()
#rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
#pickup_ac.wait_for_server() # needed?
rospy.loginfo("Connecting to grasp generator AS")
self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
#grasps_ac.wait_for_server() # needed?
#planning scene for motion planning
self.scene = PlanningSceneInterface()
# blocking action server
self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
self.feedback = GraspObjectFeedback()
self.result = GraspObjectResult()
self.current_goal = None
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_objects = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id:
goal.set_rejected() # "No objects to grasp were received on the objects topic."
return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run grasping state machine
self.grasping_sm()
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def grasping_sm(self):
if self.current_goal:
self.update_feedback("running clustering")
(object_points, obj_bbox_dims,
object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0])
#TODO visualize bbox
#TODO publish filtered pointcloud?
print obj_bbox_dims
########
self.update_feedback("check reachability")
########
self.update_feedback("generate grasps")
#transform pose to base_link
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2]/2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("no grasps received")
return
self.publish_grasps_as_poses(grasp_list)
self.feedback.grasps = grasp_list
self.current_goal.publish_feedback(self.feedback)
self.result.grasps = grasp_list
########
self.update_feedback("setup planning scene")
#remove old objects
self.scene.remove_world_object("object_to_grasp")
# add object to grasp to planning scene
self.scene.add_box("object_to_grasp", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.result.object_scene_name = "object_to_grasp"
########
if self.current_goal.get_goal().execute_grasp:
self.update_feedback("execute grasps")
pug = self.createPickupGoal("object_to_grasp", grasp_list)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Result is:")
print result
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.result.object_pose = object_pose
#bounding box in a point message
self.result.bounding_box = Point()
self.result.bounding_box.x = obj_bbox_dims[0]
self.result.bounding_box.y = obj_bbox_dims[1]
self.result.bounding_box.z = obj_bbox_dims[2]
self.current_goal.set_succeeded(result = self.result)
#self.current_goal.set_aborted()
def update_feedback(self, text):
self.feedback.last_state = text
self.current_goal.publish_feedback(self.feedback)
def update_aborted(self, text = ""):
self.update_feedback("aborted." + text)
self.current_goal.set_aborted()
def generate_grasps(self, pose, width):
#send request to block grasp generator service
if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)):
return []
rospy.loginfo("Successfully connected.")
goal = GenerateBlockGraspsGoal()
goal.pose = pose.pose
goal.width = width
self.grasps_ac.send_goal(goal)
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
t_start = rospy.Time.now()
self.grasps_ac.wait_for_result()
t_end = rospy.Time.now()
t_total = t_end - t_start
rospy.loginfo("Result received in " + str(t_total.to_sec()))
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def publish_grasps_as_poses(self, grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
self.grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"):
""" Create a PickupGoal with the provided data"""
pug = PickupGoal()
pug.target_name = target
pug.group_name = group
pug.possible_grasps.extend(possible_grasps)
pug.allowed_planning_time = 5.0
pug.planning_options.planning_scene_diff.is_diff = True
pug.planning_options.planning_scene_diff.robot_state.is_diff = True
pug.planning_options.plan_only = False
pug.planning_options.replan = True
pug.planning_options.replan_attempts = 10
pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"]
pug.allowed_touch_objects.append(target)
#pug.attached_object_touch_links.append('all')
return pug
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Ioan Sucan
import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = -0.4
p.pose.position.z = 0.85
p.pose.orientation.w = 1.0
arm.set_pose_target(p)
planed = arm.plan()
if len(planed.joint_trajectory.points) == 0:
print "useless"
attemp += 1
again = True
if again:
return None
return planed
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
# -----------------------------
# Add objecto to the scene
#------------------------------
rospy.sleep(2)
scene.remove_world_object("base")
scene.remove_attached_object("gripper_link", "box")
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = "base_link"
p.pose.position.x = 0.165
p.pose.position.y = 0.
p.pose.position.z = 0.066 - 0.115
p.pose.orientation.x = 0.707106
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_attached_object_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 设置每次运动规划的时间限制：10s
arm.set_planning_time(10)
# 移除场景中之前运行残留的物体
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
# 设置桌面的高度
table_ground = 0.6
# 设置table和tool的三维尺寸
table_size = [0.1, 0.3, 0.02]
tool_size = [0.2, 0.02, 0.02]
# 设置tool的位姿
p = PoseStamped()
p.header.frame_id = end_effector_link
p.pose.position.x = tool_size[0] / 2.0
p.pose.position.y = 0
p.pose.position.z = -0.015
p.pose.orientation.w = 1
# 将tool附着到机器人的终端
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_link'
table_pose.pose.position.x = 0.3
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
rospy.sleep(2)
# 更新当前的位姿
arm.set_start_state_to_current_state()
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [0.827228546495185, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858]
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
arm.go()
rospy.sleep(1)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_ik_demo')
# 初始化需要使用move group控制的机械臂中的arm group
rospy.Subscriber("obj_pose", Pose, obj_pose_get)
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
rospy.sleep(1)
obj_pose.position.x = 0.4
obj_pose.position.y = 0.1
obj_pose.position.z = 0.25
# 0.40 0.35 # 0.4-5
# 0 0 # 0
# 0.26 0.26 # 0
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.3, 0.01]
box1_size = [0.03, 0.001, 0.08]
# box2_size = [0.03, 0.03, 0.05]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = table_size[
0] / 2.0 + obj_pose.position.x - 0.05
table_pose.pose.position.y = obj_pose.position.y
table_pose.pose.position.z = obj_pose.position.z - table_size[
2] / 2.0 - box1_size[2] / 2.0
# table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = obj_pose.position.x
box1_pose.pose.position.y = obj_pose.position.y
box1_pose.pose.position.z = obj_pose.position.z
# box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0, 0, 0, 1)
self.setColor(box1_id, 0.8, 0, 0, 1.0)
self.setColor(box2_id, 0.8, 0, 0, 1.0)
# self.setColor(table_id, 0.8, 0, 0, 1.0) red
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0) yellow
# self.setColor(box1_id, 1, 1, 1, 1.0) white
# self.setColor(sphere_id, 0, 0, 0, 1) black
# 将场景中的颜色设置发布
self.sendColors()
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
GRIPPER_FRAME = 'right_gripper_link'
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
right_arm.set_planner_id("KPIECEkConfigDefault")
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
#rospy.logwarn("cleaning world")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("open")
#! /usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Dec 5 10:31:00 2013
@author: Sam Pfeiffer
"""
# from moveit_commander import MoveGroupCommander
from moveit_commander import RobotCommander, PlanningSceneInterface # , roscpp_initialize, roscpp_shutdown
import rospy
import actionlib
from geometry_msgs.msg import PoseStamped
from trajectory_msgs.msg import JointTrajectoryPoint
from moveit_msgs.msg import Grasp, PickupAction, PickupGoal, PickupResult
if __name__ == "__main__":
rospy.init_node("remove_world_objects")
scene = PlanningSceneInterface()
rospy.sleep(1)
rospy.loginfo("Cleaning world objects")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
class MoveGroupCommandInterpreter(object):
"""
Interpreter for simple commands
"""
DEFAULT_FILENAME = "move_group.cfg"
GO_DIRS = {
"up": (2, 1),
"down": (2, -1),
"z": (2, 1),
"left": (1, 1),
"right": (1, -1),
"y": (1, 1),
"forward": (0, 1),
"backward": (0, -1),
"back": (0, -1),
"x": (0, 1)
}
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def get_active_group(self):
if len(self._group_name) > 0:
return self._gdict[self._group_name]
else:
return None
def get_loaded_groups(self):
return self._gdict.keys()
def execute(self, cmd):
cmd = self.resolve_command_alias(cmd)
result = self.execute_generic_command(cmd)
if result != None:
return result
else:
if len(self._group_name) > 0:
return self.execute_group_command(
self._gdict[self._group_name], cmd)
else:
return (
MoveGroupInfoLevel.INFO, self.get_help() +
"\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n"
)
def execute_generic_command(self, cmd):
if os.path.isfile("cmd/" + cmd):
cmd = "load cmd/" + cmd
cmdlow = cmd.lower()
if cmdlow.startswith("use"):
if cmdlow == "use":
return (MoveGroupInfoLevel.INFO,
"\n".join(self.get_loaded_groups()))
clist = cmd.split()
clist[0] = clist[0].lower()
if len(clist) == 2:
if clist[0] == "use":
if clist[1] == "previous":
clist[1] = self._prev_group_name
if len(clist[1]) == 0:
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[1] in self._gdict:
self._prev_group_name = self._group_name
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
else:
try:
mg = MoveGroupCommander(clist[1])
self._gdict[clist[1]] = mg
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.FAIL,
"Initializing " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.FAIL,
"Unable to initialize " + clist[1])
elif cmdlow.startswith("trace"):
if cmdlow == "trace":
return (MoveGroupInfoLevel.INFO,
"trace is on" if self._trace else "trace is off")
clist = cmdlow.split()
if clist[0] == "trace" and clist[1] == "on":
self._trace = True
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[0] == "trace" and clist[1] == "off":
self._trace = False
return (MoveGroupInfoLevel.DEBUG, "OK")
elif cmdlow.startswith("load"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN,
"Unable to parse load command")
if len(clist) == 2:
filename = clist[1]
if not os.path.isfile(filename):
filename = "cmd/" + filename
line_num = 0
line_content = ""
try:
f = open(filename, 'r')
for line in f:
line_num += 1
line = line.rstrip()
line_content = line
if self._trace:
print("%s:%d: %s" %
(filename, line_num, line_content))
comment = line.find("#")
if comment != -1:
line = line[0:comment].rstrip()
if line != "":
self.execute(line.rstrip())
line_content = ""
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
if line_num > 0:
return (MoveGroupInfoLevel.WARN,
"Error at %s:%d: %s\n%s" %
(filename, line_num, line_content, str(e)))
else:
return (MoveGroupInfoLevel.WARN,
"Processing " + filename + ": " + str(e))
except:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" %
(filename, line_num, line_content))
else:
return (MoveGroupInfoLevel.WARN,
"Unable to load " + filename)
elif cmdlow.startswith("save"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN,
"Unable to parse save command")
if len(clist) == 2:
filename = clist[1]
try:
f = open(filename, 'w')
for gr in self._gdict.keys():
f.write("use " + gr + "\n")
known = self._gdict[gr].get_remembered_joint_values()
for v in known.keys():
f.write(v + " = [" +
" ".join([str(x) for x in known[v]]) + "]\n")
if self._db_host != None:
f.write("database " + self._db_host + " " +
str(self._db_port) + "\n")
return (MoveGroupInfoLevel.DEBUG, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
else:
return None
def execute_group_command(self, g, cmdorig):
cmd = cmdorig.lower()
if cmd == "stop":
g.stop()
return (MoveGroupInfoLevel.DEBUG, "OK")
if cmd == "id":
return (MoveGroupInfoLevel.INFO, g.get_name())
if cmd == "help":
return (MoveGroupInfoLevel.INFO, self.get_help())
if cmd == "vars":
known = g.get_remembered_joint_values()
return (MoveGroupInfoLevel.INFO,
"[" + " ".join(known.keys()) + "]")
if cmd == "joints":
joints = g.get_joints()
return (MoveGroupInfoLevel.INFO, "\n" + "\n".join(
[str(i) + " = " + joints[i]
for i in range(len(joints))]) + "\n")
if cmd == "show":
return self.command_show(g)
if cmd == "current":
return self.command_current(g)
if cmd == "ground":
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
# offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
pose.pose.position.z = -0.0501
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = self._robot.get_root_link()
self._planning_scene_interface.attach_box(
self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
return (MoveGroupInfoLevel.INFO, "Added ground")
if cmd == "eef":
if len(g.get_end_effector_link()) > 0:
return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
else:
return (MoveGroupInfoLevel.INFO,
"There is no end effector defined")
if cmd == "database":
if self._db_host == None:
return (MoveGroupInfoLevel.INFO, "Not connected to a database")
else:
return (MoveGroupInfoLevel.INFO, "Connected to " +
self._db_host + ":" + str(self._db_port))
if cmd == "plan":
if self._last_plan != None:
return (MoveGroupInfoLevel.INFO, str(self._last_plan))
else:
return (MoveGroupInfoLevel.INFO, "No previous plan")
if cmd == "constrain":
g.clear_path_constraints()
return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
if cmd == "tol" or cmd == "tolerance":
return (MoveGroupInfoLevel.INFO,
"Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" %
g.get_goal_tolerance())
if cmd == "time":
return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
if cmd == "execute":
if self._last_plan != None:
if g.execute(self._last_plan):
return (MoveGroupInfoLevel.SUCCESS,
"Plan submitted for execution")
else:
return (MoveGroupInfoLevel.WARN,
"Failed to submit plan for execution")
else:
return (MoveGroupInfoLevel.WARN, "No motion plan computed")
# see if we have assignment between variables
assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
if assign_match:
known = g.get_remembered_joint_values()
if assign_match.group(2) in known:
g.remember_joint_values(assign_match.group(1),
known[assign_match.group(2)])
return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) +
" is now the same as " + assign_match.group(2))
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
# see if we have assignment of matlab-like vector syntax
set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
if set_match:
try:
g.remember_joint_values(
set_match.group(1),
[float(x) for x in set_match.group(2).split()])
return (MoveGroupInfoLevel.SUCCESS,
"Remembered joint values [" + set_match.group(2) +
"] under the name " + set_match.group(1))
except:
return (MoveGroupInfoLevel.WARN,
"Unable to parse joint value [" + set_match.group(2) +
"]")
# see if we have assignment of matlab-like element update
component_match = re.match(
r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
if component_match:
known = g.get_remembered_joint_values()
if component_match.group(1) in known:
try:
val = known[component_match.group(1)]
val[int(component_match.group(2))] = float(
component_match.group(3))
g.remember_joint_values(component_match.group(1), val)
return (MoveGroupInfoLevel.SUCCESS,
"Updated " + component_match.group(1) + "[" +
component_match.group(2) + "]")
except:
return (MoveGroupInfoLevel.WARN,
"Unable to parse index or value in '" + cmd + "'")
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
clist = cmdorig.split()
clist[0] = clist[0].lower()
# if this is an unknown one-word command, it is probably a variable
if len(clist) == 1:
known = g.get_remembered_joint_values()
if cmd in known:
return (MoveGroupInfoLevel.INFO,
"[" + " ".join([str(x) for x in known[cmd]]) + "]")
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
# command with one argument
if len(clist) == 2:
if clist[0] == "go":
self._last_plan = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
if g.go():
return (MoveGroupInfoLevel.SUCCESS,
"Moved to random target [" +
" ".join([str(x) for x in vals]) + "]")
else:
return (MoveGroupInfoLevel.FAIL,
"Failed while moving to random target [" +
" ".join([str(x) for x in vals]) + "]")
else:
try:
g.set_named_target(clist[1])
if g.go():
return (MoveGroupInfoLevel.SUCCESS,
"Moved to " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL,
"Failed while moving to " + clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN,
"Going to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN,
clist[1] + " is unknown")
if clist[0] == "plan":
self._last_plan = None
vals = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
self._last_plan = g.plan()[1] # The trajectory msg
else:
try:
g.set_named_target(clist[1])
self._last_plan = g.plan()[1] # The trajectory msg
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN,
"Planning to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN,
clist[1] + " is unknown")
if self._last_plan != None:
if len(self._last_plan.joint_trajectory.points
) == 0 and len(
self._last_plan.multi_dof_joint_trajectory.
points) == 0:
self._last_plan = None
dest_str = clist[1]
if vals != None:
dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
if self._last_plan != None:
return (MoveGroupInfoLevel.SUCCESS,
"Planned to " + dest_str)
else:
return (MoveGroupInfoLevel.FAIL,
"Failed while planning to " + dest_str)
elif clist[0] == "pick":
self._last_plan = None
if g.pick(clist[1]):
return (MoveGroupInfoLevel.SUCCESS,
"Picked object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL,
"Failed while trying to pick object " + clist[1])
elif clist[0] == "place":
self._last_plan = None
if g.place(clist[1]):
return (MoveGroupInfoLevel.SUCCESS,
"Placed object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL,
"Failed while trying to place object " + clist[1])
elif clist[0] == "planner":
g.set_planner_id(clist[1])
return (MoveGroupInfoLevel.SUCCESS,
"Planner is now " + clist[1])
elif clist[0] == "record" or clist[0] == "rec":
g.remember_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS,
"Remembered current joint values under the name " +
clist[1])
elif clist[0] == "rand" or clist[0] == "random":
g.remember_joint_values(clist[1], g.get_random_joint_values())
return (MoveGroupInfoLevel.SUCCESS,
"Remembered random joint values under the name " +
clist[1])
elif clist[0] == "del" or clist[0] == "delete":
g.forget_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS,
"Forgot joint values under the name " + clist[1])
elif clist[0] == "show":
known = g.get_remembered_joint_values()
if clist[1] in known:
return (MoveGroupInfoLevel.INFO,
"[" + " ".join([str(x)
for x in known[clist[1]]]) + "]")
else:
return (MoveGroupInfoLevel.WARN,
"Joint values for " + clist[1] + " are not known")
elif clist[0] == "tol" or clist[0] == "tolerance":
try:
g.set_goal_tolerance(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN,
"Unable to parse tolerance value '" + clist[1] +
"'")
elif clist[0] == "time":
try:
g.set_planning_time(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN,
"Unable to parse planning duration value '" +
clist[1] + "'")
elif clist[0] == "constrain":
try:
g.set_path_constraints(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
if self._db_host != None:
return (MoveGroupInfoLevel.WARN,
"Constraint " + clist[1] + " is not known.")
else:
return (MoveGroupInfoLevel.WARN,
"Not connected to a database.")
elif clist[0] == "wait":
try:
time.sleep(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS,
clist[1] + " seconds passed")
except:
return (MoveGroupInfoLevel.WARN,
"Unable to wait '" + clist[1] + "' seconds")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], self._db_port)
self._db_host = clist[1]
return (MoveGroupInfoLevel.SUCCESS, "Connected to " +
self._db_host + ":" + str(self._db_port))
except:
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" +
clist[1] + ":" + str(self._db_port) + "'")
else:
return (MoveGroupInfoLevel.WARN,
"Unknown command: '" + cmd + "'")
if len(clist) == 3:
if clist[0] == "go" and clist[1] in self.GO_DIRS:
self._last_plan = None
try:
offset = float(clist[2])
(axis, factor) = self.GO_DIRS[clist[1]]
return self.command_go_offset(g, offset, factor, axis,
clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN,
"Going " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN,
"Unable to parse distance '" + clist[2] + "'")
elif clist[0] == "allow" and (clist[1] == "look"
or clist[1] == "looking"):
if clist[2] == "1" or clist[2] == "true" or clist[
2] == "T" or clist[2] == "True":
g.allow_looking(True)
else:
g.allow_looking(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "allow" and (clist[1] == "replan"
or clist[1] == "replanning"):
if clist[2] == "1" or clist[2] == "true" or clist[
2] == "T" or clist[2] == "True":
g.allow_replanning(True)
else:
g.allow_replanning(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], int(clist[2]))
self._db_host = clist[1]
self._db_port = int(clist[2])
return (MoveGroupInfoLevel.SUCCESS, "Connected to " +
self._db_host + ":" + str(self._db_port))
except:
self._db_host = None
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" +
clist[1] + ":" + clist[2] + "'")
if len(clist) == 4:
if clist[0] == "rotate":
try:
g.set_rpy_target([float(x) for x in clist[1:]])
if g.go():
return (MoveGroupInfoLevel.SUCCESS,
"Rotation complete")
else:
return (MoveGroupInfoLevel.FAIL,
"Failed while rotating to " +
" ".join(clist[1:]))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN,
"Unable to parse X-Y-Z rotation values '" +
" ".join(clist[1:]) + "'")
if len(clist) >= 7:
if clist[0] == "go":
self._last_plan = None
approx = False
if (len(clist) > 7):
if (clist[7] == "approx" or clist[7] == "approximate"):
approx = True
try:
p = Pose()
p.position.x = float(clist[1])
p.position.y = float(clist[2])
p.position.z = float(clist[3])
q = tf.transformations.quaternion_from_euler(
float(clist[4]), float(clist[5]), float(clist[6]))
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
if approx:
g.set_joint_value_target(p, True)
else:
g.set_pose_target(p)
if g.go():
return (MoveGroupInfoLevel.SUCCESS,
"Moved to pose target\n%s\n" % (str(p)))
else:
return (MoveGroupInfoLevel.FAIL,
"Failed while moving to pose \n%s\n" %
(str(p)))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN,
"Going to pose target: %s" % (str(e)))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" +
" ".join(clist[1:]) + "'")
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
def command_show(self, g):
known = g.get_remembered_joint_values()
res = []
for k in known.keys():
res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
return (MoveGroupInfoLevel.INFO, "\n".join(res))
def command_current(self, g):
res = "joints = [" + " ".join(
[str(x) for x in g.get_current_joint_values()]) + "]"
if len(g.get_end_effector_link()) > 0:
res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(
g.get_current_pose()) + " ]"
res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(
g.get_current_rpy())
return (MoveGroupInfoLevel.INFO, res)
def command_go_offset(self, g, offset, factor, dimension_index,
direction_name):
if g.has_end_effector_link():
try:
g.shift_pose_target(dimension_index, factor * offset)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved " +
direction_name + " by " + str(offset) + " m")
else:
return (MoveGroupInfoLevel.FAIL,
"Failed while moving " + direction_name)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN,
"Unable to process pose update")
else:
return (MoveGroupInfoLevel.WARN,
"No known end effector. Cannot move " + direction_name)
def resolve_command_alias(self, cmdorig):
cmd = cmdorig.lower()
if cmd == "which":
return "id"
if cmd == "groups":
return "use"
return cmdorig
def get_help(self):
res = []
res.append("Known commands:")
res.append(" help show this screen")
res.append(
" allow looking <true|false> enable/disable looking around")
res.append(
" allow replanning <true|false> enable/disable replanning")
res.append(" constrain clear path constraints")
res.append(
" constrain <name> use the constraint <name> as a path constraint"
)
res.append(
" current show the current state of the active group")
res.append(
" database display the current database connection (if any)"
)
res.append(
" delete <name> forget the joint values under the name <name>"
)
res.append(
" eef print the name of the end effector attached to the current group"
)
res.append(
" execute execute a previously computed motion plan")
res.append(
" go <name> plan and execute a motion to the state <name>"
)
res.append(
" go rand plan and execute a motion to a random state"
)
res.append(
" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>"
)
res.append(
" ground add a ground plane to the planning scene")
res.append(
" id|which display the name of the group that is operated on"
)
res.append(
" joints display names of the joints in the active group"
)
res.append(
" load [<file>] load a set of interpreted commands from a file"
)
res.append(" pick <name> pick up object <name>")
res.append(" place <name> place object <name>")
res.append(" plan <name> plan a motion to the state <name>")
res.append(" plan rand plan a motion to a random state")
res.append(
" planner <name> use planner <name> to plan next motion")
res.append(
" record <name> record the current joint values under the name <name>"
)
res.append(
" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)"
)
res.append(
" save [<file>] save the currently known variables as a set of commands"
)
res.append(
" show display the names and values of the known states"
)
res.append(" show <name> display the value of a state")
res.append(" stop stop the active group")
res.append(
" time show the configured allowed planning time")
res.append(" time <val> set the allowed planning time")
res.append(
" tolerance show the tolerance for reaching the goal region"
)
res.append(
" tolerance <val> set the tolerance for reaching the goal region"
)
res.append(
" trace <on|off> enable/disable replanning or looking around"
)
res.append(
" use <name> switch to using the group named <name> (and load it if necessary)"
)
res.append(
" use|groups show the group names that are already loaded"
)
res.append(
" vars display the names of the known states")
res.append(" wait <dt> sleep for <dt> seconds")
res.append(" x = y assign the value of y to x")
res.append(" x = [v1 v2...] assign a vector of values to x")
res.append(
" x[idx] = val assign a value to dimension idx of x")
return "\n".join(res)
def get_keywords(self):
known_vars = []
known_constr = []
if self.get_active_group() != None:
known_vars = self.get_active_group().get_remembered_joint_values(
).keys()
known_constr = self.get_active_group().get_known_constraints()
groups = self._robot.get_group_names()
return {
'go':
['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] +
known_vars,
'help': [],
'record':
known_vars,
'show':
known_vars,
'wait': [],
'delete':
known_vars,
'database': [],
'current': [],
'use':
groups,
'load': [],
'save': [],
'pick': [],
'place': [],
'plan':
known_vars,
'allow': ['replanning', 'looking'],
'constrain':
known_constr,
'vars': [],
'joints': [],
'tolerance': [],
'time': [],
'eef': [],
'execute': [],
'ground': [],
'id': []
}
class MoveGroupCommandInterpreter(object):
"""
Interpreter for simple commands
"""
DEFAULT_FILENAME = "move_group.cfg"
GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1),
"left" : (1, 1), "right" : (1, -1), "y" : (1, 1),
"forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) }
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def get_active_group(self):
if len(self._group_name) > 0:
return self._gdict[self._group_name]
else:
return None
def get_loaded_groups(self):
return self._gdict.keys()
def execute(self, cmd):
cmd = self.resolve_command_alias(cmd)
result = self.execute_generic_command(cmd)
if result != None:
return result
else:
if len(self._group_name) > 0:
return self.execute_group_command(self._gdict[self._group_name], cmd)
else:
return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n")
def execute_generic_command(self, cmd):
if os.path.isfile("cmd/" + cmd):
cmd = "load cmd/" + cmd
cmdlow = cmd.lower()
if cmdlow.startswith("use"):
if cmdlow == "use":
return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups()))
clist = cmd.split()
clist[0] = clist[0].lower()
if len(clist) == 2:
if clist[0] == "use":
if clist[1] == "previous":
clist[1] = self._prev_group_name
if len(clist[1]) == 0:
return (MoveGroupInfoLevel.DEBUG, "OK")
if self._gdict.has_key(clist[1]):
self._prev_group_name = self._group_name
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
else:
try:
mg = MoveGroupCommander(clist[1])
self._gdict[clist[1]] = mg
self._group_name = clist[1]
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1])
elif cmdlow.startswith("trace"):
if cmdlow == "trace":
return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off")
clist = cmdlow.split()
if clist[0] == "trace" and clist[1] == "on":
self._trace = True
return (MoveGroupInfoLevel.DEBUG, "OK")
if clist[0] == "trace" and clist[1] == "off":
self._trace = False
return (MoveGroupInfoLevel.DEBUG, "OK")
elif cmdlow.startswith("load"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN, "Unable to parse load command")
if len(clist) == 2:
filename = clist[1]
if not os.path.isfile(filename):
filename = "cmd/" + filename
line_num = 0
line_content = ""
try:
f = open(filename, 'r')
for line in f:
line_num += 1
line = line.rstrip()
line_content = line
if self._trace:
print ("%s:%d: %s" % (filename, line_num, line_content))
comment = line.find("#")
if comment != -1:
line = line[0:comment].rstrip()
if line != "":
self.execute(line.rstrip())
line_content = ""
return (MoveGroupInfoLevel.DEBUG, "OK")
except MoveItCommanderException as e:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e)))
else:
return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e))
except:
if line_num > 0:
return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content))
else:
return (MoveGroupInfoLevel.WARN, "Unable to load " + filename)
elif cmdlow.startswith("save"):
filename = self.DEFAULT_FILENAME
clist = cmd.split()
if len(clist) > 2:
return (MoveGroupInfoLevel.WARN, "Unable to parse save command")
if len(clist) == 2:
filename = clist[1]
try:
f = open(filename, 'w')
for gr in self._gdict.keys():
f.write("use " + gr + "\n")
known = self._gdict[gr].get_remembered_joint_values()
for v in known.keys():
f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n")
if self._db_host != None:
f.write("database " + self._db_host + " " + str(self._db_port) + "\n")
return (MoveGroupInfoLevel.DEBUG, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to save " + filename)
else:
return None
def execute_group_command(self, g, cmdorig):
cmd = cmdorig.lower()
if cmd == "stop":
g.stop()
return (MoveGroupInfoLevel.DEBUG, "OK")
if cmd == "id":
return (MoveGroupInfoLevel.INFO, g.get_name())
if cmd == "help":
return (MoveGroupInfoLevel.INFO, self.get_help())
if cmd == "vars":
known = g.get_remembered_joint_values()
return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")
if cmd == "joints":
joints = g.get_joints()
return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")
if cmd == "show":
return self.command_show(g)
if cmd == "current":
return self.command_current(g)
if cmd == "ground":
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
# offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
pose.pose.position.z = -0.0501
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = self._robot.get_root_link()
self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
return (MoveGroupInfoLevel.INFO, "Added ground")
if cmd == "eef":
if len(g.get_end_effector_link()) > 0:
return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
else:
return (MoveGroupInfoLevel.INFO, "There is no end effector defined")
if cmd == "database":
if self._db_host == None:
return (MoveGroupInfoLevel.INFO, "Not connected to a database")
else:
return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
if cmd == "plan":
if self._last_plan != None:
return (MoveGroupInfoLevel.INFO, str(self._last_plan))
else:
return (MoveGroupInfoLevel.INFO, "No previous plan")
if cmd == "constrain":
g.clear_path_constraints()
return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
if cmd == "tol" or cmd == "tolerance":
return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())
if cmd == "time":
return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
if cmd == "execute":
if self._last_plan != None:
if g.execute(self._last_plan):
return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution")
else:
return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution")
else:
return (MoveGroupInfoLevel.WARN, "No motion plan computed")
# see if we have assignment between variables
assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd)
if assign_match:
known = g.get_remembered_joint_values()
if known.has_key(assign_match.group(2)):
g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)])
return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2))
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
# see if we have assignment of matlab-like vector syntax
set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd)
if set_match:
try:
g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()])
return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]")
# see if we have assignment of matlab-like element update
component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd)
if component_match:
known = g.get_remembered_joint_values()
if known.has_key(component_match.group(1)):
try:
val = known[component_match.group(1)]
val[int(component_match.group(2))] = float(component_match.group(3))
g.remember_joint_values(component_match.group(1), val)
return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
clist = cmdorig.split()
clist[0] = clist[0].lower()
# if this is an unknown one-word command, it is probably a variable
if len(clist) == 1:
known = g.get_remembered_joint_values()
if known.has_key(cmd):
return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
# command with one argument
if len(clist) == 2:
if clist[0] == "go":
self._last_plan = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]")
else:
try:
g.set_named_target(clist[1])
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
if clist[0] == "plan":
self._last_plan = None
vals = None
if clist[1] == "rand" or clist[1] == "random":
vals = g.get_random_joint_values()
g.set_joint_value_target(vals)
self._last_plan = g.plan()
else:
try:
g.set_named_target(clist[1])
self._last_plan = g.plan()
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown")
if self._last_plan != None:
if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0:
self._last_plan = None
dest_str = clist[1]
if vals != None:
dest_str = "[" + " ".join([str(x) for x in vals]) + "]"
if self._last_plan != None:
return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str)
else:
return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str)
elif clist[0] == "pick":
self._last_plan = None
if g.pick(clist[1]):
return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1])
elif clist[0] == "place":
self._last_plan = None
if g.place(clist[1]):
return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1])
else:
return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1])
elif clist[0] == "planner":
g.set_planner_id(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1])
elif clist[0] == "record" or clist[0] == "rec":
g.remember_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1])
elif clist[0] == "rand" or clist[0] == "random":
g.remember_joint_values(clist[1], g.get_random_joint_values())
return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1])
elif clist[0] == "del" or clist[0] == "delete":
g.forget_joint_values(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1])
elif clist[0] == "show":
known = g.get_remembered_joint_values()
if known.has_key(clist[1]):
return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]")
else:
return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known")
elif clist[0] == "tol" or clist[0] == "tolerance":
try:
g.set_goal_tolerance(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'")
elif clist[0] == "time":
try:
g.set_planning_time(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'")
elif clist[0] == "constrain":
try:
g.set_path_constraints(clist[1])
return (MoveGroupInfoLevel.SUCCESS, "OK")
except:
if self._db_host != None:
return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.")
else:
return (MoveGroupInfoLevel.WARN, "Not connected to a database.")
elif clist[0] == "wait":
try:
time.sleep(float(clist[1]))
return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed")
except:
return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], self._db_port)
self._db_host = clist[1]
return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
except:
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'")
else:
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
if len(clist) == 3:
if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]):
self._last_plan = None
try:
offset = float(clist[2])
(axis, factor) = self.GO_DIRS[clist[1]]
return self.command_go_offset(g, offset, factor, axis, clist[1])
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'")
elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"):
if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
g.allow_looking(True)
else:
g.allow_looking(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"):
if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True":
g.allow_replanning(True)
else:
g.allow_replanning(False)
return (MoveGroupInfoLevel.DEBUG, "OK")
elif clist[0] == "database":
try:
g.set_constraints_database(clist[1], int(clist[2]))
self._db_host = clist[1]
self._db_port = int(clist[2])
return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port))
except:
self._db_host = None
return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'")
if len(clist) == 4:
if clist[0] == "rotate":
try:
g.set_rpy_target([float(x) for x in clist[1:]])
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Rotation complete")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation values '" + " ".join(clist[1:]) + "'")
if len(clist) >= 7:
if clist[0] == "go":
self._last_plan = None
approx = False
if (len(clist) > 7):
if (clist[7] == "approx" or clist[7] == "approximate"):
approx = True
try:
p = Pose()
p.position.x = float(clist[1])
p.position.y = float(clist[2])
p.position.z = float(clist[3])
q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6]))
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
if approx:
g.set_joint_value_target(p, True)
else:
g.set_pose_target(p)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)))
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)))
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)))
except:
return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'")
return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'")
def command_show(self, g):
known = g.get_remembered_joint_values()
res = []
for k in known.keys():
res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]")
return (MoveGroupInfoLevel.INFO, "\n".join(res))
def command_current(self, g):
res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]"
if len(g.get_end_effector_link()) > 0:
res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]"
res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())
return (MoveGroupInfoLevel.INFO, res)
def command_go_offset(self, g, offset, factor, dimension_index, direction_name):
if g.has_end_effector_link():
try:
g.shift_pose_target(dimension_index, factor * offset)
if g.go():
return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m")
else:
return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name)
except MoveItCommanderException as e:
return (MoveGroupInfoLevel.WARN, str(e))
except:
return (MoveGroupInfoLevel.WARN, "Unable to process pose update")
else:
return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name)
def resolve_command_alias(self, cmdorig):
cmd = cmdorig.lower()
if cmd == "which":
return "id"
if cmd == "groups":
return "use"
return cmdorig
def get_help(self):
res = []
res.append("Known commands:")
res.append(" help show this screen")
res.append(" id|which display the name of the group that is operated on")
res.append(" load [<file>] load a set of interpreted commands from a file")
res.append(" save [<file>] save the currently known variables as a set of commands")
res.append(" use <name> switch to using the group named <name> (and load it if necessary)")
res.append(" use|groups show the group names that are already loaded")
res.append(" vars display the names of the known states")
res.append(" show display the names and values of the known states")
res.append(" show <name> display the value of a state")
res.append(" record <name> record the current joint values under the name <name>")
res.append(" delete <name> forget the joint values under the name <name>")
res.append(" current show the current state of the active group")
res.append(" constrain <name> use the constraint <name> as a path constraint")
res.append(" constrain clear path constraints")
res.append(" eef print the name of the end effector attached to the current group")
res.append(" go <name> plan and execute a motion to the state <name>")
res.append(" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>")
res.append(" go rand plan and execute a motion to a random state")
res.append(" plan <name> plan a motion to the state <name>")
res.append(" execute execute a previously computed motion plan")
res.append(" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)")
res.append(" tolerance show the tolerance for reaching the goal region")
res.append(" tolerance <val> set the tolerance for reaching the goal region")
res.append(" wait <dt> sleep for <dt> seconds")
res.append(" x = y assign the value of y to x")
res.append(" x[idx] = val assign a value to dimension idx of x")
res.append(" x = [v1 v2...] assign a vector of values to x")
res.append(" trace <on|off> enable/disable replanning or looking around")
res.append(" ground add a ground plane to the planning scene")
res.append(" allow replanning <true|false> enable/disable replanning")
res.append(" allow looking <true|false> enable/disable looking around")
return "\n".join(res)
def get_keywords(self):
known_vars = []
known_constr = []
if self.get_active_group() != None:
known_vars = self.get_active_group().get_remembered_joint_values().keys()
known_constr = self.get_active_group().get_known_constraints()
groups = self._robot.get_group_names()
return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars,
'help':[],
'record':known_vars,
'show':known_vars,
'wait':[],
'delete':known_vars,
'database': [],
'current':[],
'use':groups,
'load':[],
'save':[],
'pick':[],
'place':[],
'plan':known_vars,
'allow':['replanning', 'looking'],
'constrain':known_constr,
'vars':[],
'joints':[],
'tolerance':[],
'time':[],
'eef':[],
'execute':[],
'ground':[],
'id':[]}
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# DAVES Initialize the move group for the right gripper
right_gripper = MoveGroupCommander('left_gripper')
# Open the gripper to the neutral position
right_gripper.set_joint_value_target([-0.5]) #radians
# rospy.loginfo("=============> DEBUG EXCEPT: Ignoring set_joint_value_target error")
right_gripper.go()
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01) # 0.01
right_arm.set_goal_orientation_tolerance(0.1) # 0.05
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_home')
right_arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.45 # 0.75
# DAVES ADDED
table_offset_x = 0.30 # meters (centemeters / 100)
table_offset_y = -0.30 # meters (positive = left)
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.26 + table_offset_x
table_pose.pose.position.y = table_offset_y
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.21 + table_offset_x
box1_pose.pose.position.y = -0.1 + table_offset_y
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.19 + table_offset_x
box2_pose.pose.position.y = 0.15 + table_offset_y
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.2 + table_offset_x
target_pose.pose.position.y = table_offset_y
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_home')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('SceneSetup')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Set the reference frame for pose targets
reference_frame = 'rack1'
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(bowl_id)
scene.remove_world_object(pitcher_id)
# Set the height of the table off the ground
table_ground = 0.5762625
# Set the length, width and height of the table and boxes
table_size = [1.0128, 0.481, 0.0238125]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0
table_pose.pose.position.y = 0.847725
table_pose.pose.position.z = table_ground
# Set the height of the bowl
bowl_ground = 0.57816875
bowl_pose = PoseStamped()
bowl_pose.header.frame_id = reference_frame
bowl_pose.pose.position.x = 0
bowl_pose.pose.position.y = 0.847725
bowl_pose.pose.position.z = bowl_ground
# Set the height of the pitcher
pitcher_ground = 0.57816875
pitcher_pose = PoseStamped()
pitcher_pose.header.frame_id = reference_frame
pitcher_pose.pose.position.x = 0.4
pitcher_pose.pose.position.y = 0.847725
pitcher_pose.pose.position.z = pitcher_ground
pitcher_pose.pose.orientation.w = -0.5
pitcher_pose.pose.orientation.z = 0.707
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0.4, 0, 1.0)
self.setColor(bowl_id, 0, 0.4, 0.8, 1.0)
self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0)
self.sendColors()
scene.add_box(table_id, table_pose, table_size)
scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
#scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class PickAndPlaceServer(object):
def __init__(self):
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
'''rospy.loginfo("Connecting to place AS")
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")'''
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy(
'/get_planning_scene', GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy(
'/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
def set_height(height):
self.object_height = height.data
rospy.loginfo("Object height set to " + str(self.object_height))
rospy.Subscriber("height", Float64, set_height)
# Get the object size
self.object_height = rospy.get_param('~object_height')
self.object_width = rospy.get_param('~object_width')
self.object_depth = rospy.get_param('~object_depth')
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
else:
rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer(
'/pickup_pose', PickUpPoseAction,
execute_cb=self.pick_cb, auto_start=False)
self.pick_as.start()
'''self.place_as = SimpleActionServer(
'/place_pose', PickUpPoseAction,
execute_cb=self.place_cb, auto_start=False)
self.place_as.start()'''
def pick_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.grasp_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.pick_as.set_aborted(p_res)
else:
self.pick_as.set_succeeded(p_res)
'''def place_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.place_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.place_as.set_aborted(p_res)
else:
self.place_as.set_succeeded(p_res)'''
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo(
"Waiting for object '" + object_name + "'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
#HEARTS tried inputting multiple cubes to approximate cylinder
#TODO re-investigate if time allows
#def rotate_quat(self,quat,angle_deg,axis_euler):
'''
Rotate about axis_euler by angle_deg,the quaternion quat,
returned as a quaternion
'''
#euler = tf.transformations.euler_from_quaternion(quat)
#euler(2) = euler(0.5*np.pi)
def grasp_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
rospy.loginfo("Adding new 'part' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
part_pose = copy.deepcopy(object_pose)
part_pose.pose.position.z -= self.object_height/4
#Add object description in scene
self.scene.add_box("part", part_pose, (self.object_depth, self.object_width,
self.object_height))
# Add rotated boxes (for collision avoidance)
#box1_pose = copy.deepcopy(object_pose)
rospy.loginfo("Second%s", object_pose.pose)
table_pose = copy.deepcopy(object_pose)
#define a virtual table below the object
table_height = object_pose.pose.position.z - self.object_height
table_width = 1.8
table_depth = 0.5
#TODO update so it works when detecting centre of object
table_pose.pose.position.z += -(self.object_height)/2 -table_height/2
table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("table", table_pose, (table_depth, table_width, table_height))
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object("part")
self.wait_for_planning_scene_object("table")
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
self.pickup_ac
goal = createPickupGoal(
"arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact)
rospy.loginfo("Waiting for final pick instruction")
rospy.wait_for_message('/pick_it',String)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo(
"Pick result: " +
str(moveit_error_dict[result.error_code.val]))
return result.error_code.val
'''def place_object(self, object_pose):
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.65
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.55
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.54
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.60
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.50
place_pose.pose.position.y = -0.25
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Initialize the move group for the right arm
Arm = MoveGroupCommander(GROUP_NAME_ARM)
Arm.set_planner_id("RRTstarkConfigDefault")
# Initialize the move group for the right gripper
Hand = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = Arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
Arm.set_goal_position_tolerance(0.05)
Arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
Arm.allow_replanning(True)
# Set the right arm reference frame
Arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
Arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 3
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
target_id = 'target'
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Open the gripper to the neutral position
#Hand.set_joint_value_target(GRIPPER_NEUTRAL)
#Hand.go()
#rospy.sleep(1)
target_size = [0.01, 0.01, 0.01]
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.3
target_pose.pose.position.y = 0.03
target_pose.pose.position.z = -0.3
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Initialize the grasp pose to the target pose
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = REFERENCE_FRAME
grasp_pose.pose = target_pose.pose
grasp_pose.pose.position.y = 0.2
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = Arm.pick(target_id, grasps)
rospy.sleep(0.2)
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
arm = MoveGroupCommander(PLANNING_GROUP)
# # tolerance
# arm.set_goal_position_tolerance(0.01)
# arm.set_goal_orientation_tolerance(0.01)
# arm.set_goal_joint_tolerance(0.01)
# arm.set_goal_tolerance(0.01)
# Action Clients for Place
rospy.loginfo("Connecting to place AS")
place_ac = actionlib.SimpleActionClient('/place', PlaceAction)
place_ac.wait_for_server()
rospy.loginfo("Successfully connected")
# create Planning Scene
scene = PlanningSceneInterface()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = REFERENCE_LINK
p.header.stamp = rospy.Time.now()
quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
p.pose.orientation = Quaternion(*quat)
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# add table
scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2))
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪张开
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 设置桌面的高度
table_ground = 0.2
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.45
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.41
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.39
box2_pose.pose.position.y = 0.13
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# 将桌子设置成蓝色，两个box设置成橙色
self.setColor(table_id, 0, 0, 0.8, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.02, 0.01, 0.12]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.42
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.42
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪回到张开的状态
gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
### Helper function
def gen_pose(frame_id="/base_link", pos=[0,0,0], euler=[0,0,0]):
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.header.stamp = rospy.Time.now()
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = pos
pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(*euler)
return pose
if __name__ == '__main__':
rospy.init_node('scripting_example')
while rospy.get_time() == 0.0: pass
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rospy.sleep(1.0)
### Create a handle for the Move Group Commander
mgc = MoveGroupCommander("arm")
rospy.sleep(1.0)
### Add virtual obstacle
pose = gen_pose(pos=[-0.2, -0.1, 1.2])
psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
rospy.sleep(1.0)
### Move to stored joint position
mgc.set_named_target("left")
mgc.go()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the lwr_kuka4plus
lwr_kuka4plus = MoveGroupCommander('lwr_kuka4plus')
# Get the name of the end-effector link
end_effector_link = lwr_kuka4plus.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
lwr_kuka4plus.set_goal_position_tolerance(0.01)
lwr_kuka4plus.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
lwr_kuka4plus.allow_replanning(True)
# Set the reference frame for pose targets
reference_frame = 'lwr_base_link'
# Set the lwr_kuka4plus reference frame accordingly
lwr_kuka4plus.set_pose_reference_frame(reference_frame)
# Allow 5 seconds per planning attempt
lwr_kuka4plus.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "home" pose stored in the SRDF file
lwr_kuka4plus.set_named_target('home')
lwr_kuka4plus.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.46
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.41
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.39
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.2
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[
2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
lwr_kuka4plus.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
lwr_kuka4plus.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "home" pose stored in the SRDF file
lwr_kuka4plus.set_named_target('home')
lwr_kuka4plus.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
eef_pose.pose.orientation.x = heh[0]
eef_pose.pose.orientation.y = heh[1]
eef_pose.pose.orientation.z = heh[2]
eef_pose.pose.orientation.w = heh[3]
eef_pose = listener.transformPose('world',eef_pose)
break
count = count+1
rate.sleep()
return eef_pose
if __name__ == '__main__':
rospy.init_node('spawnobject')
scene = PlanningSceneInterface()
br = tf.TransformBroadcaster()
listener = tf.TransformListener()
robot = MoveGroupCommander("sia5d");
gripper = MoveGroupCommander("gripper");
rospy.sleep(1)
p = PoseStamped()
p1 = PoseStamped()
p2 = PoseStamped()
bowl2eef = PoseStamped()
p_goal = PoseStamped()
p.header.frame_id = "world"
p1.header.frame_id = "world"
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown, MoveGroupCommander
from geometry_msgs.msg import PoseStamped
import moveit_msgs.msg
import geometry_msgs.msg
import moveit_commander
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('move_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
group = MoveGroupCommander("manipulator")
# specify the planner
group.set_planner_id("RRTkConfigDefault")
rospy.sleep(3)
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
# display_trajectory_publisher = rospy.Publisher(
# '/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory)
class PickAruco(object):
def __init__(self):
rospy.loginfo("Initalizing...")
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
time.sleep(1.0)
if not self.pick_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /pickup_pose AS")
exit()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher('/torso_controller/command',
JointTrajectory,
queue_size=1)
self.head_cmd = rospy.Publisher('/head_controller/command',
JointTrajectory,
queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Done initializing PickAruco.")
self.pick_g = None
self.move_pub = rospy.Publisher('/mobile_base_controller/cmd_vel',
Twist,
queue_size=1)
self.scene = PlanningSceneInterface()
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def pick_aruco(self, string_operation):
#rospy.sleep(2.0)
if string_operation == "pick":
self.prepare_robot()
# Detect object
rospy.loginfo(
"spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.pose.orientation = aruco_pose.pose.orientation
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# rospy.loginfo("Moving arm to a safe pose")
# pmg = PlayMotionGoal()
# pmg.motion_name = 'pick_final_pose'
# pmg.skip_planning = False
# self.play_m_as.send_goal_and_wait(pmg)
# rospy.loginfo("Raise object done.")
# Move to safe
home = False
i = 0
#self.move_back(5)
while not home and i < 3:
home = self.move_arm_home()
i += 1
rospy.sleep(0.5)
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return TriggerResponse(False, "Failed to pick")
self.pick_g = pick_g
return TriggerResponse(True, "Succeeded")
elif string_operation == "place":
if not self.pick_g:
rospy.logerr("Failed to place, no object position known")
return TriggerResponse(
False, "Failed to place, no object position known")
pick_g = self.pick_g
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'prepare_grasp' #'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Place the object back to its position
rospy.loginfo("Gonna place near where it was")
pick_g.object_pose.pose.position.z += 0.05
pick_g.object_pose.pose.position.y -= 0.35
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# Move to safe
home = False
i = 0
#self.move_back(5)
while not home and i < 3:
home = self.move_arm_home()
i += 1
rospy.sleep(0.5)
result = self.place_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to place")
return TriggerResponse(False, "Failed to place")
self.pick_g = None
return TriggerResponse(True, "Succeeded")
def lift_torso(self):
rospy.loginfo("Moving torso up")
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.34]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.torso_cmd.publish(jt)
def lower_head(self):
rospy.loginfo("Moving head down")
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, -0.75]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.loginfo("Done.")
def prepare_robot(self):
rospy.loginfo("Unfold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'pregrasp'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done.")
self.lower_head()
rospy.loginfo("Robot prepared.")
def move_back(self, n):
for i in range(n):
self.move_pub.publish(Twist(linear=Vector3(-0.15, 0, 0)))
rospy.sleep(0.05)
def move_arm_home(self):
rospy.loginfo("Fold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'home'
pmg.skip_planning = False
res = self.play_m_as.send_goal_and_wait(pmg)
if res != GoalStatus.SUCCEEDED:
return False
self.scene.remove_world_object("table")
rospy.loginfo("Done.")
return True
class ObjectManipulationAS:
def __init__(self, name):
# stuff for grasp planning
rospy.loginfo("Getting a TransformListener...")
self.tf_listener = tf.TransformListener()
rospy.loginfo("Getting a TransformBroadcaster...")
self.tf_broadcaster = tf.TransformBroadcaster()
rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_clusters = None
rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)
if DEBUG_MODE:
self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)
rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
self.grasps_ac.wait_for_server()
rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
self.depth_service.wait_for_service()
rospy.loginfo("Getting a PlanningSceneInterface instance...")
self.scene = PlanningSceneInterface()
# blocking action server
rospy.loginfo("Creating Action Server '" + name + "'...")
self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
self.as_feedback = ObjectManipulationFeedback()
self.as_result = ObjectManipulationActionResult()
self.current_goal = None
# Take care of left and right arm grasped stuff
self.right_hand_object = None
self.left_hand_object = None
self.current_side = 'right'
rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
self.grasp_obj_as.start()
def objects_callback(self, data):
rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects))
self.last_clusters = data
def goal_callback(self, goal):
if self.current_goal:
goal.set_rejected() # "Server busy"
return
# TODO: Check if pose is not empty, if it is, reject
# elif len(self.last_clusters.objects) - 1 < goal.get_goal().target_id:
# goal.set_rejected() # "No objects to grasp were received on the objects topic."
# return
else:
#store and accept new goal
self.current_goal = goal
self.current_goal.set_accepted()
#run the corresponding operation
if self.current_goal.get_goal().operation == ObjectManipulationGoal.PICK:
rospy.loginfo("ObjectManipulation: PICK!")
self.pick_operation()
elif self.current_goal.get_goal().operation == ObjectManipulationGoal.PLACE:
rospy.loginfo("ObjectManipulation: PLACE!")
self.place_operation()
else:
rospy.logerr("ObjectManipulation: Erroneous operation!")
#finished, get rid of goal
self.current_goal = None
def cancel_callback(self, goal):
#TODO stop motions?
self.current_goal.set_canceled()
def pick_operation(self):
"""Execute pick operation"""
if self.message_fields_ok():
self.as_result = ObjectManipulationResult()
goal_message_field = self.current_goal.get_goal()
self.update_feedback("Checking hand to use")
# Check which arm group was requested and if it's currently free of objects
if 'right' in goal_message_field.group:
if self.right_hand_object: # Something already in the hand
self.update_aborted("Right hand already contains an object.")
return # necessary?
self.current_side = 'right'
elif 'left' in goal_message_field.group:
if self.left_hand_object:
self.update_aborted("Left hand already contains an object.")
return
self.current_side = 'left'
# Publish pose of goal position
if DEBUG_MODE:
self.to_grasp_object_pose_pub.publish(goal_message_field.target_pose)
self.update_feedback("Detecting clusters")
if not self.wait_for_recognized_array(wait_time=5, timeout_time=10): # wait until we get clusters published
self.update_aborted("Failed detecting clusters")
# Search closer cluster
# transform pose to base_link if needed
if goal_message_field.target_pose.header.frame_id != "base_link":
self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
object_to_grasp_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
else:
object_to_grasp_pose = goal_message_field.target_pose.pose
self.update_feedback("Searching closer cluster while clustering")
(closest_cluster_id, (object_points, obj_bbox_dims, object_bounding_box, object_pose)) = self.get_id_of_closest_cluster_to_pose(object_to_grasp_pose)
rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id))
#TODO visualize bbox
#TODO publish filtered pointcloud?
rospy.logdebug("BBOX: " + str(obj_bbox_dims))
########
self.update_feedback("Check reachability")
########
self.update_feedback("Generating grasps")
rospy.logdebug("Object pose before tf thing is: " + str(object_pose))
#transform pose to base_link, IS THIS NECESSARY?? should be a function in any case
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
#HACK remove orientation -> pose is aligned with parent(base_link)
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
# shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects)
# TODO remove this hack, fix it in table filtering
object_pose.pose.position.z += obj_bbox_dims[2] / 2.0
grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x
# check if there are grasps, if not, abort
if len(grasp_list) == 0:
self.update_aborted("No grasps received")
return
if DEBUG_MODE: # TODO: change to dynamic param
publish_grasps_as_poses(grasp_list)
self.current_goal.publish_feedback(self.as_feedback)
########
self.update_feedback("Setup planning scene")
# Add object to grasp to planning scene
self.scene.add_box(self.current_side + "_hand_object", object_pose,
(obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2]))
self.as_result.object_scene_name = self.current_side + "_hand_object"
########
self.update_feedback("Execute grasps")
pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group)
rospy.loginfo("Sending pickup goal")
self.pickup_ac.send_goal(pug)
rospy.loginfo("Waiting for result...")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
########
self.update_feedback("finished")
self.as_result.object_pose = object_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if self.current_side == 'right':
self.right_hand_object = self.current_side + "_hand_object"
elif self.current_side == 'left':
self.left_hand_object = self.current_side + "_hand_object"
self.current_goal.set_succeeded(result=self.as_result)
else:
# Remove object as it has not been picked
self.scene.remove_world_object(self.current_side + "_hand_object")
self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def place_operation(self):
"""Execute the place operation"""
if self.message_fields_ok():
self.as_result = ObjectManipulationResult()
goal_message_field = self.current_goal.get_goal()
self.update_feedback("Checking arm to use")
# Check which arm group was requested and if it currently has an object
if 'right' in goal_message_field.group:
if not self.right_hand_object: # Something already in the hand
self.update_aborted("Right hand does not contain an object.")
return # necessary?
self.current_side = 'right'
current_target = self.right_hand_object
elif 'left' in goal_message_field.group:
if not self.left_hand_object:
self.update_aborted("Left hand does not contain an object.")
return
self.current_side = 'left'
current_target = self.left_hand_object
# transform pose to base_link if needed
if goal_message_field.target_pose.header.frame_id != "base_link":
self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5))
placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose)
else:
placing_pose = goal_message_field.target_pose.pose
#### TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE ####
self.update_feedback("Sending place order to MoveIt!")
placing_pose = PoseStamped(header=Header(frame_id="base_link"), pose=placing_pose)
goal = createPlaceGoal(placing_pose, group=goal_message_field.group, target=current_target)
rospy.loginfo("Sending place goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result...")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val]))
self.as_result.object_pose = placing_pose
self.as_result.error_code = result.error_code
self.as_result.error_message = str(moveit_error_dict[result.error_code.val])
if result.error_code.val == MoveItErrorCodes.SUCCESS:
# Emptying hand
self.update_feedback("Emptying hand")
if self.current_side == 'right':
self.as_result.object_scene_name = current_target
self.right_hand_object = None
elif self.current_side == 'left':
self.as_result.object_scene_name = current_target
self.left_hand_object = None
# Removing object from the collision world
self.scene.remove_world_object(current_target)
self.current_goal.set_succeeded(result=self.as_result)
else:
self.update_aborted(text="MoveIt place failed", manipulation_result=self.as_result)
else:
self.update_aborted("Goal fields not correctly fulfilled")
def message_fields_ok(self):
"""Check if the minimal fields for the message are fulfilled"""
if self.current_goal:
goal_message_field = self.current_goal.get_goal()
if len(goal_message_field.group) == 0:
rospy.logwarn("Group field empty.")
return False
if goal_message_field.operation != ObjectManipulationGoal.PICK and goal_message_field.operation != ObjectManipulationGoal.PLACE:
rospy.logwarn("Asked for an operation different from PICK or PLACE: " + str(goal_message_field.operation))
return False
if len(goal_message_field.target_pose.header.frame_id) == 0:
rospy.logwarn("Target pose frame_id is empty")
return False
return True
def update_feedback(self, text):
"""Publish feedback with given text"""
self.as_feedback.last_state = text
self.current_goal.publish_feedback(self.as_feedback)
def update_aborted(self, text="", manipulation_result=None):
"""Publish feedback and abort with the text give and optionally an ObjectManipulationResult already fulfilled"""
self.update_feedback("aborted." + text)
if manipulation_result == None:
aborted_result = ObjectManipulationResult()
aborted_result.error_message = text
self.current_goal.set_aborted(result=aborted_result)
else:
self.current_goal.set_aborted(result=manipulation_result)
def generate_grasps(self, pose, width):
"""Send request to block grasp generator service"""
goal = GenerateGraspsGoal()
goal.pose = pose.pose
goal.width = width
grasp_options = GraspGeneratorOptions()
grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y
grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN
grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF
goal.options.append(grasp_options)
self.grasps_ac.send_goal(goal)
if DEBUG_MODE:
rospy.loginfo("Sent goal, waiting:\n" + str(goal))
self.grasps_ac.wait_for_result()
grasp_list = self.grasps_ac.get_result().grasps
return grasp_list
def get_id_of_closest_cluster_to_pose(self, input_pose):
"""Returns the id of the closest cluster to the pose provided (in Pose or PoseStamped)
and all the associated clustering information"""
current_id = 0
closest_pose = None
closest_object_points = None
closest_id = 0
closest_bbox = None
closest_bbox_dims = None
closest_distance = 99999.9
for myobject in self.last_clusters.objects:
(object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(myobject.point_clouds[0])
self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15))
trans_pose = self.tf_listener.transformPose("base_link", object_pose)
object_pose = trans_pose
rospy.loginfo("id: " + str(current_id) + "\n pose:\n" + str(object_pose))
if closest_pose == None: # first loop iteration
closest_object_points = object_points
closest_pose = object_pose
closest_bbox = object_bounding_box
closest_bbox_dims = obj_bbox_dims
closest_distance = dist_between_poses(closest_pose, input_pose)
else:
if dist_between_poses(object_pose, input_pose) < closest_distance:
closest_object_points = object_points
closest_distance = dist_between_poses(object_pose, input_pose)
closest_pose = object_pose
closest_bbox = object_bounding_box
closest_bbox_dims = obj_bbox_dims
closest_id = current_id
current_id += 1
rospy.loginfo("Closest id is: " + str(closest_id) + " at " + str(closest_pose))
return closest_id, (closest_object_points, closest_bbox_dims, closest_bbox_dims, closest_pose)
def wait_for_recognized_array(self, wait_time=6, timeout_time=10):
"""Ask for depth images until we get a recognized array
wait for wait_time between depth throtle calls
stop if timeout_time is achieved
If we dont find it in the correspondent time return false, true otherwise"""
initial_time = rospy.Time.now()
self.last_clusters = None
count = 0
num_calls = 1
self.depth_service.call(EmptyRequest())
rospy.loginfo("Depth throtle server call #" + str(num_calls))
rospy.loginfo("Waiting for a recognized array...")
while rospy.Time.now() - initial_time < rospy.Duration(timeout_time) and self.last_clusters == None:
rospy.sleep(0.1)
count += 1
if count >= wait_time / 10:
self.depth_service.call(EmptyRequest())
num_calls += 1
rospy.loginfo("Depth throtle server call #" + str(num_calls))
if self.last_clusters == None:
return False
else:
return True
import copy
import random
from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GenerateGraspsResult
moveit_error_dict = {}
for name in MoveItErrorCodes.__dict__.keys():
if not name[:1] == '_':
code = MoveItErrorCodes.__dict__[name]
moveit_error_dict[code] = name
if __name__=='__main__':
rospy.init_node("part2_attacher")
scene = PlanningSceneInterface()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = '/base_link'
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.45
p.pose.orientation.w = 1.0
#scene.add_box("table", p, (0.5, 1.5, 0.9))
p.pose.position.x = 0.45
p.pose.position.y = -0.1
p.pose.position.z = 1.0
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_ik_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('manipulator')
# 设置目标位置所使用的参考坐标系
reference_frame = 'kinect_frame_optical'
arm.set_pose_reference_frame(reference_frame)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
arm.set_planning_time(10)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('forward')
arm.go()
rospy.sleep(1)
#===============加入墙面====================
# 移除场景中之前运行残留的物体
scene.remove_world_object('wall')
# 设置wall的三维尺寸
wall_size = [2, 2, 0.001]
# 将wall加入场景当中
wall_pose = PoseStamped()
wall_pose.header.frame_id = 'wall'
wall_pose.pose.position.x = 0
wall_pose.pose.position.y = 0
wall_pose.pose.position.z = 0
wall_pose.pose.orientation.w = 1.0
scene.add_box('wall', wall_pose, wall_size)
rospy.sleep(2)
#=========移动到起点状态=============
# 更新当前的位姿
arm.set_start_state_to_current_state()
#从参数服务器读取墙面信息
wall_tf = rospy.get_param("wall_tf")
wall_coeff = rospy.get_param("wall_coeff")#墙面方程参数ABCD
# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
# 姿态使用四元数描述，基于base_link坐标系
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = -0.35
target_pose.pose.position.y = 0.42
#由平面方程求Z并-0.2
target_pose.pose.position.z = -(wall_coeff[0]*target_pose.pose.position.x+wall_coeff[1]*target_pose.pose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
target_pose.pose.orientation.x = wall_tf[3]
target_pose.pose.orientation.y = wall_tf[4] #-0.70729
target_pose.pose.orientation.z = wall_tf[5]
target_pose.pose.orientation.w = wall_tf[6] # 0.70729
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
# 规划运动路径
traj = arm.plan()
# 按照规划的运动路径控制机械臂运动
arm.execute(traj)
rospy.sleep(1)
#=================设置路径点====================================
# 初始化路点列表
waypoints = []
wpose = deepcopy(target_pose.pose)
# 将初始位姿加入路点列表
waypoints.append(deepcopy(wpose))
# 设置路点数据，并加入路点列表
wpose.position.y -= 0.2
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.2
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.y -= 0.2
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.2
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.y -= 0.2
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.2
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.y -= 0.2
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.2
wpose.position.z = -(wall_coeff[0]*wpose.position.x+wall_coeff[1]*wpose.position.y+wall_coeff[3])/wall_coeff[2]-0.2
waypoints.append(deepcopy(wpose))
#=====================================================
#笛卡尔路径规划
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path (
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
#=====================================================
# 控制机械臂回到初始化位置
arm.set_named_target('forward')
arm.go()
rospy.sleep(1)
# 移除场景中之前运行残留的物体
scene.remove_world_object('wall')
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
if __name__ == "__main__":
roscpp_initialize(sys.argv)
rospy.init_node("moveit_py_demo", anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
rospy.logwarn("cleaning world")
# right_arm.set_named_target("r_start")
# right_arm.go()
# right_gripper.set_named_target("open")
# right_gripper.go()
rospy.sleep(3)
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')
#!/usr/bin/env python
import rospy, sys, tf
from onine_arm import Arm
from geometry_msgs.msg import *
from tf.transformations import quaternion_from_euler, euler_from_quaternion
from moveit_commander import RobotCommander, PlanningSceneInterface
import moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('tilt_eef', anonymous=True)
scene = PlanningSceneInterface()
rospy.sleep(2)
scene.remove_world_object("target")
onine_arm = Arm()
onine_arm.tilt_food()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene = PlanningSceneInterface()
pub_traj = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.gripperCtrl = rospy.ServiceProxy(
"/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
self.colors = dict()
rospy.sleep(1)
self.robot = moveit_commander.robot.RobotCommander()
self.arm = MoveGroupCommander('arm')
self.arm.allow_replanning(True)
cartesian = rospy.get_param('~cartesian', True)
self.arm_end_effector_link = self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names = [[]]
self.joint_names[0] = rospy.get_param('controller_joint_names')
self.traj = trajectory_msgs.msg.JointTrajectory()
self.sub_jpc = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#scene planning
self.l_id = 'l_tool'
self.r_id = 'r_tool'
self.table_id = 'table'
self.target1_id = 'target1_object'
self.target2_id = 'target2_object'
#~ self.target3_id='target3_object'
#~ self.target4_id='target4_object'
self.f_target_id = 'receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
#~ self.scene.remove_world_object(self.target3_id)
#~ self.scene.remove_world_object(self.target4_id)
self.scene.remove_world_object(self.f_target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.table_ground = 0.13
self.table_size = [0.9, 0.6, 0.018]
self.setupSence()
target1_size = [0.035, 0.035, 0.19]
target2_size = target1_size
#~ target3_size=target1_size
#~ target4_size=target1_size
self.f_target_size = [0.2, 0.2, 0.04]
f_target_pose = PoseStamped()
pre_pour_pose = PoseStamped()
target1_pose = PoseStamped()
target2_pose = PoseStamped()
#~ target3_pose=PoseStamped()
#~ target4_pose=PoseStamped()
joint_names = [
'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
]
joint_names = rospy.get_param('controller_joint_names')
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = joint_names
#final target
l_gripper = 0.18
#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
#~ self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1)
#~ self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#target localization
msg1 = rospy.wait_for_message('/aruco_simple/pose', Pose)
rospy.sleep(1)
msg2 = rospy.wait_for_message('/aruco_simple/pose2', Pose)
rospy.sleep(1)
self.add_target(f_target_pose, self.f_target_size,
self.reference_frame, -msg2.position.y - 0.257,
-msg2.position.x + 0.81 - 0.1 - l_gripper, 0, 0, 0, 1)
self.scene.add_box(self.f_target_id, f_target_pose, self.f_target_size)
self.add_target(target1_pose, target1_size, self.reference_frame,
-msg1.position.y - 0.257,
-msg1.position.x + 0.81 - 0.065, 0, 0, 0, 1)
self.scene.add_box(self.target1_id, target1_pose, target1_size)
#~ self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1)
#~ self.scene.add_box(self.target2_id,target2_pose,target2_size)
#~ self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1)
#~ self.scene.add_box(self.target3_id,target3_pose,target3_size)
#~ self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1)
#~ self.scene.add_box(self.target4_id,target4_pose,target4_size)
#attach_pose
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
self.setColor(self.target1_id, 0.8, 0, 0, 1.0)
self.setColor(self.target2_id, 0.8, 0, 0, 1.0)
#~ self.setColor(self.target3_id,0.8,0,0,1.0)
#self.setColor(self.target4_id,0.8,0,0,1.0)
self.setColor(self.f_target_id, 0.8, 0.3, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(3)
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
j_ori_state = [
-1.2628753185272217, -0.442996621131897, -0.1326361745595932,
2.333048105239868, -0.15598002076148987, -1.2167049646377563,
3.1414425373077393
]
#signal= True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
#parameter setup
tar_num = 1
maxtries = 300
r_grasp = 0.16
#~ topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"]
#~ save_data=["carrot.txt","pepper.txt","cucumber.txt"]
#ratio_table=[0.03,0.025,0.04]
for i in range(0, tar_num):
#grasp target
rospy.loginfo("Choosing source...")
#~ dfile=open(save_data[i],"a")
if i == 0:
target_pose = target1_pose
target_id = self.target1_id
target_size = target1_size
#~ amp=0.13
#~ freq=2.75
#~ r_angle=40.0
elif i == 1:
target_pose = target2_pose
target_id = self.target2_id
target_size = target2_size
#~ amp=0.15
#~ freq=2.5
#~ r_angle=45.0
elif i == 2:
target_pose = target3_pose
target_id = self.target3_id
target_size = target3_size
amp = 0.15
freq = 2.75
r_angle = 40.0
elif i == 3:
target_pose = target4_pose
target_id = self.target4_id
target_size = target4_size
amp = 0.1
freq = 2.75
r_angle = 45.0
r_pour = 0.13
r_bottle = 0.1
h_pour = 0.1
pour_angle = [0.0, -15.0, 15.0, -30.0, 30.0, -45.0, 45.0]
#~ tip_angle=[10.0,20.0,30.0,40.0,50.0,60.0,70.0,80.0,90.0]
#~ tip_angle=[15.0,30.0,45.0,60.0,75.0,90.0]
tip_angle = [20.0, 30.0, 40.0, 50.0, 60.0, 70.00]
shake_freq = [2.0, 2.25, 2.50, 2.75]
shake_amp = [0.05, 0.075, 0.1, 0.125, 0.15]
shake_per_times = 5
shake_times_tgt = 2
rospy.loginfo("Params loaded.")
rospy.loginfo("Current Source: " + str(i + 1) + " ")
#grasp and back
ori_pose, g_pose = self.pg_g_pp(target_pose, r_grasp)
ori_joint_state = self.arm.get_current_joint_values()
#move to target position for pouring
for m in range(len(pour_angle)):
for j in range(len(tip_angle)):
for k in range(len(shake_freq)):
dfile = open(
"pepper_tipangle_" + str(tip_angle[j]) +
"_shaking_freq_" + str(shake_freq[k]) + "_1", "a")
for n in range(len(shake_amp)):
pp_ps_sgn = self.pp_ps(f_target_pose,
pour_angle[m], r_pour,
h_pour)
if pp_ps_sgn == 0:
rospy.loginfo(
"pre-Pouring angle not correct, back for replanning..."
)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
initial_pose = self.arm.get_current_pose(
self.arm_end_effector_link)
pour_signal = self.pour_rotate(
initial_pose, pour_angle[m], tip_angle[j],
r_bottle, maxtries)
if pour_signal != 1:
rospy.loginfo(
"Pouring angle not correct, back for replanning..."
)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
amp = shake_amp[n]
freq = shake_freq[k]
rospy.loginfo("shaking degree : " +
str(tip_angle[j]) +
", shaking amp : " + str(amp) +
", shaking freq : " + str(freq))
shake_times = 0
signal = True
start_joint_state = self.arm.get_current_joint_values(
)
signal, end_joint_state = self.shake_a(
pour_angle[m], tip_angle[j], amp)
while signal == 1:
#~ area_ratio= rospy.wait_for_message('/ratio_carrot',Float64)
#if (area_ratio>ratio_table[i]) or (shake_times>shake_times_tgt):
if (shake_times < shake_times_tgt):
self.shaking(start_joint_state,
end_joint_state, freq,
shake_per_times)
rospy.sleep(2)
shake_times += 1
rospy.loginfo("shaking times : " +
str(shake_times) + " .")
area_ratio = rospy.wait_for_message(
'/ratio_carrot', Float64)
if shake_times == shake_times_tgt:
dfile.write(
"%f %f \r\n" %
(shake_amp[n], area_ratio.data))
self.rotate_back(ori_joint_state)
rospy.sleep(2)
else:
signal = 0
self.rotate_back(ori_joint_state)
rospy.sleep(2)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
#remove and shut down
self.scene.remove_attached_object(self.arm_end_effector_link, 'l_tool')
self.scene.remove_attached_object(self.arm_end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.2) #0.1
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'pick_target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_home')
right_arm.go()
# Open the gripper to the neutral position
try:
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
except:
rospy.loginfo("=============> DEBUG EXCEPT: Ignoring set_joint_value_target error")
try:
right_gripper.go()
except:
rospy.loginfo("=============> DEBUG EXCEPT: Ignoring right_gripper.go error")
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.45 # 0.75 # DAVES
# DAVES ADDED
table_offset_x = 0.30 # meters (centemeters / 100)
table_offset_y = -0.30 # meters (positive = left)
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.25 + table_offset_x
table_pose.pose.position.y = 0.0 + table_offset_y
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = 0.21 + table_offset_x
box1_pose.pose.position.y = -0.1 + table_offset_y
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = 0.19 + table_offset_x
box2_pose.pose.position.y = 0.13 + table_offset_y
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.22 + table_offset_x
target_pose.pose.position.y = 0.0 + table_offset_y
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
rospy.loginfo("=============> DEBUG Target and table setup done")
rospy.sleep(3)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.18 + table_offset_x
place_pose.pose.position.y = -0.18 + table_offset_y
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_arm_home')
right_arm.go()
# Open the gripper to the neutral position
# Open the gripper to the neutral position
try:
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
except:
rospy.loginfo("=============> DEBUG EXCEPT: Ignoring set_joint_value_target error")
try:
right_gripper.go()
except:
rospy.loginfo("=============> DEBUG EXCEPT: Ignoring right_gripper.go error")
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene=PlanningSceneInterface()
pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10)
#pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
self.colors=dict()
rospy.sleep(1)
self.robot=moveit_commander.robot.RobotCommander()
self.arm=MoveGroupCommander('arm')
self.arm.allow_replanning(True)
cartesian=rospy.get_param('~cartesian',True)
self.arm_end_effector_link=self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame='base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names= [[]]
self.joint_names[0]= rospy.get_param('controller_joint_names')
self.traj= trajectory_msgs.msg.JointTrajectory()
self.sub_jpc= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory,queue_size=10)
#scene planning
self.l_id='l_tool'
self.r_id='r_tool'
self.table_id='table'
self.target1_id='target1_object'
self.target2_id='target2_object'
self.target3_id='target3_object'
#self.target4_id='target4_object'
self.f_target_id='receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
self.scene.remove_world_object(self.target3_id)
#self.scene.remove_world_object(self.target4_id)
self.scene.remove_world_object(self.f_target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.table_ground=0.13
self.table_size=[0.9,0.6,0.018]
self.setupSence()
target1_size=[0.035,0.035,0.19]
target2_size=target1_size
target3_size=target1_size
#target4_size=target1_size
self.f_target_size=[0.2,0.2,0.04]
f_target_pose=PoseStamped()
pre_pour_pose=PoseStamped()
target1_pose=PoseStamped()
target2_pose=PoseStamped()
target3_pose=PoseStamped()
#target4_pose=PoseStamped()
joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')]
joint_names= rospy.get_param('controller_joint_names')
traj= trajectory_msgs.msg.JointTrajectory()
traj.joint_names= joint_names
#final target
l_gripper=0.18
#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1)
self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#target localization
msg1 = rospy.wait_for_message('/aruco_simple/pose',Pose)
rospy.sleep(2)
msg2 = rospy.wait_for_message('/aruco_simple/pose2',Pose)
self.add_target(target1_pose,target1_size,self.reference_frame,-msg1.position.y-0.257,-msg1.position.x+0.81-0.065,0,0,0,1)
self.scene.add_box(self.target1_id,target1_pose,target1_size)
self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1)
self.scene.add_box(self.target2_id,target2_pose,target2_size)
self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1)
self.scene.add_box(self.target3_id,target3_pose,target3_size)
#self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1)
#self.scene.add_box(self.target4_id,target4_pose,target4_size)
#attach_pose
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y=-0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
#set color
self.setColor(self.target1_id,0.8,0,0,1.0)
self.setColor(self.target2_id,0.8,0,0,1.0)
self.setColor(self.target3_id,0.8,0,0,1.0)
#self.setColor(self.target4_id,0.8,0,0,1.0)
self.setColor(self.f_target_id,0.8,0.3,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(3)
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
j_ori_state=[-1.2628753185272217, -0.442996621131897, -0.1326361745595932, 2.333048105239868, -0.15598002076148987, -1.2167049646377563, 3.1414425373077393]
#signal= True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
#parameter setup
tar_num=3
maxtries=300
r_grasp=0.15
topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"]
save_data=["ratio_carrot.txt","ratio_lettuce.txt","ratio_pepper.txt"]
ratio_table=[0.03,0.025,0.04]
for i in range(0,tar_num):
#grasp target
rospy.loginfo("Choosing source...")
dfile=open(save_data[i],"a")
if i==0:
target_pose=target1_pose
target_id=self.target1_id
target_size=target1_size
amp=0.13
freq=2.75
r_angle=40.0
elif i==1:
target_pose=target2_pose
target_id=self.target2_id
target_size=target2_size
amp=0.15
freq=2.5
r_angle=45.0
elif i==2:
target_pose=target3_pose
target_id=self.target3_id
target_size=target3_size
amp=0.15
freq=2.75
r_angle=40.0
elif i==3:
target_pose=target4_pose
target_id=self.target4_id
target_size=target4_size
amp=0.1
freq=2.75
r_angle=45.0
r_pour=0.1
r_bottle=0.1
h_pour=0.1
#pour_angle=[15.0,30.0,45.0,60.0,75.0]
pour_angle=[-15.0,-10.0,-5.0,0.0,10.0,15.0,30.0,45.0,60.0,75.0]
rospy.loginfo("Params loaded.")
rospy.loginfo("Current Source: "+str(i+1)+" ")
#grasp and back
ori_pose,g_pose=self.pg_g_pp(target_pose,r_grasp)
#move to target position for pouring
pre_p_angle=self.pp_ps(f_target_pose,pour_angle,r_pour,h_pour)
rospy.loginfo("pre_pour_angle : "+str(pre_p_angle))
#rotation and shaking
ps_signal=self.pour_rotate(pre_p_angle,r_angle,r_bottle,maxtries)
if ps_signal !=1:
rospy.loginfo("Pre_shaking_rotation failed, back to ori_pose...")
self.go_back(ori_pose,g_pose)
rospy.sleep(5)
continue
else:
rospy.loginfo("Shaking planning...")
shake_per_times=3
shake_times=0
shake_times_tgt=5
signal=True
rospy.loginfo("Parameter loaded")
rospy.sleep(1)
start_shake_pose=self.arm.get_current_pose(self.arm_end_effector_link)# for trajectory of shaking
start_joint_state=self.arm.get_current_joint_values() # for state[6] to rotate
shift_pose=deepcopy(start_shake_pose)
r_angle=(r_angle/180.0)*3.14
pre_p_angle=(pre_p_angle/180.0)*3.14
shift_pose.pose.position.x+=amp*math.sin(r_angle)*math.cos(pre_p_angle)# in verticle direction
shift_pose.pose.position.y-=amp*math.sin(r_angle)*math.sin(pre_p_angle)
shift_pose.pose.position.z+=amp*math.cos(r_angle)#...
if cartesian:
signal,end_joint_state=self.cts(start_shake_pose,shift_pose,300)
#shaking process
while signal==1:
area_ratio= rospy.wait_for_message(topic_name[i],Float64)
dfile.write("%f %f\r\n" % (shake_times,area_ratio.data))
if (area_ratio.data<ratio_table[i]) and (shake_times<shake_times_tgt):
#if (shake_times<shake_times_tgt):
start_joint_state_1=self.arm.get_current_joint_values()
self.shaking(start_joint_state_1,end_joint_state,freq,shake_per_times)
shake_times+=1
rospy.loginfo("shaking times : "+str(shake_times)+ " .")
else:
signal=0
self.rotate_back(r_angle)
self.go_back(ori_pose,g_pose)
rospy.sleep(2)
continue
#remove and shut down
self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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)
plan, fraction = cartesian_path(-x_distance, -y_distance, -z_distance)
move_group.execute(plan, wait=True)
rospy.sleep(3)
(result_xyz, result_rot) = get_TF('/odom', 'ee_link')
print 'xyz_result=', result_xyz[0], result_xyz[1], result_xyz[2]
print "Grasping complete!, Go to home pose..!"
if __name__ == '__main__':
#GROUP_NAME_GRIPPER = "NAME OF GRIPPER"
roscpp_initialize(sys.argv)
rospy.init_node('control_Husky_UR3', anonymous=True)
robot = RobotCommander()
scene = PlanningSceneInterface()
##모바일 파트 관련 변수 선언
x = 0.0
y = 0.0
theta = 0.0
## 매니퓰레이터 변수 선언
group_name = "ur3_manipulator"
move_group = MoveGroupCommander(group_name)
FIXED_FRAME = 'world'
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory, queue_size=20)
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)
# clean the scene
#scene.remove_world_object("block1")
#scene.remove_world_object("block2")
#scene.remove_world_object("block3")
#scene.remove_world_object("block4")
#scene.remove_world_object("table")
#scene.remove_world_object("bowl")
#scene.remove_world_object("box")
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.7
p.pose.position.y = -0.4
p.pose.position.z = 0.85
p.pose.orientation.w = 1.57
scene.add_box("block1", p, (0.044, 0.044, 0.044))
p.pose.position.y = -0.2
p.pose.position.z = 0.175
class PickAndPlace:
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self, traj, time, positions, velocities=None):
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions = copy.deepcopy(positions)
if velocities is not None:
point.velocities = copy.deepcopy(velocities)
point.time_from_start = rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self, q_traj, t_traj):
assert (len(q_traj) == len(t_traj))
#Insert current position to beginning.
if t_traj[0] > 1.0e-2:
t_traj.insert(0, 0.0)
q_traj.insert(0, self.Q(arm=arm))
self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj)
def QTrajToDQTraj(self, q_traj, t_traj):
dof = len(q_traj[0])
#Modeling the trajectory with spline.
splines = [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj = []
for t in t_traj:
dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
#print dq_traj
return dq_traj
def JointNames(self):
#0arm= 0
return self.joint_names[0]
def ROSGetJTP(self, q, t, dq=None):
jp = trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions = q
jp.time_from_start = rospy.Duration(t)
if dq is not None: jp.velocities = dq
return jp
def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None):
assert (len(q_traj) == len(t_traj))
if dq_traj is not None: (len(dq_traj) == len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names = joint_names
if dq_traj is not None:
self.traj.points = [
self.ROSGetJTP(q, t, dq)
for q, t, dq in zip(q_traj, t_traj, dq_traj)
]
else:
self.traj.points = [
self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj)
]
self.traj.header.stamp = rospy.Time.now()
#print self.traj
return self.traj
def SmoothQTraj(self, q_traj):
if len(q_traj) == 0: return
q_prev = np.array(q_traj[0])
q_offset = np.array([0] * len(q_prev))
for q in q_traj:
q_diff = np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d] < -math.pi: q_offset[d] += 1
elif q_diff[d] > math.pi: q_offset[d] -= 1
q_prev = copy.deepcopy(q)
q[:] = q + q_offset * 2.0 * math.pi
def add_target(self, target_pose, target_size, frame, x, y, o1, o2, o3,
o4):
target_pose.header.frame_id = frame
target_pose.pose.position.x = x
target_pose.pose.position.y = y
target_pose.pose.position.z = self.table_ground + self.table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.x = o1
target_pose.pose.orientation.y = o2
target_pose.pose.orientation.z = o3
target_pose.pose.orientation.w = o4
#self.scene.add_box(f_target_id,f_target_pose,f_target_size)
def cts(self, start_pose, end_pose, maxtries, exe_signal=False):
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
for i in range(2, len(plan.joint_trajectory.points)):
q_traj.append(
plan.joint_trajectory.points[i].positions)
t_traj.append(t_traj[-1] + 0.08)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = 1
return signal, end_joint_state
#move and rotate
def cts_rotate(self,
start_pose,
end_pose,
angle_r,
maxtries,
exe_signal=False):
angle = angle_r * 3.14 / 180.0
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0.0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
per_angle = angle / (len(plan.joint_trajectory.points) - 2)
for i in range(2, len(plan.joint_trajectory.points)):
joint_inc_list = [
j
for j in plan.joint_trajectory.points[i].positions
]
#plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
joint_inc_list[6] -= per_angle * (i - 1)
q_traj.append(joint_inc_list)
t_traj.append(t_traj[-1] + 0.07)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = 1
return signal, end_joint_state
def cts_rotate_delta(self,
start_pose,
end_pose,
angle_pre,
angle_r,
maxtries,
exe_signal=False):
angle = (angle_r - angle_pre) * 3.14 / 180.0
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0.0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
end_joint_state = plan.joint_trajectory.points[
-1].positions
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
per_angle = angle / (len(plan.joint_trajectory.points) - 2)
for i in range(2, len(plan.joint_trajectory.points)):
joint_inc_list = [
j
for j in plan.joint_trajectory.points[i].positions
]
#~ plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
#~ if i==len(plan.joint_trajectory.points)-1:
#~ joint_inc_list[6]-=angle
#~ q_traj.append(joint_inc_list)
#~ t_traj.append(1.5)
joint_inc_list[6] -= per_angle * (i - 1) + (
angle_pre * 3.14 / 180.0)
q_traj.append(joint_inc_list)
t_traj.append(t_traj[-1] + 0.1)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
signal = 1
return signal, end_joint_state
# shaking function:
# freq : shaking freqence
# times : shaking time per action
def shaking(self, initial_state, end_joint_state, freq, times):
q_traj = [initial_state]
t_traj = [0.0]
for i in range(times):
q_traj.append(end_joint_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
q_traj.append(initial_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(6)
def shake_a(self, pre_p_angle, r_angle, amp):
start_shake_pose = self.arm.get_current_pose(
self.arm_end_effector_link) # for trajectory of shaking
start_joint_state = self.arm.get_current_joint_values(
) # for state[6] to rotate
shift_pose = deepcopy(start_shake_pose)
r_angle = (r_angle / 180.0) * 3.14
pre_p_angle = (pre_p_angle / 180.0) * 3.14
shift_pose.pose.position.x += amp * math.sin(r_angle) * math.cos(
pre_p_angle) # in verticle direction
shift_pose.pose.position.y -= amp * math.sin(r_angle) * math.sin(
pre_p_angle)
shift_pose.pose.position.z += amp * math.cos(r_angle) #...
signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
return signal, end_joint_state
def shake_b(self, pre_p_angle, r_angle, amp):
start_shake_pose = self.arm.get_current_pose(
self.arm_end_effector_link) # for trajectory of shaking
start_joint_state = self.arm.get_current_joint_values(
) # for state[6] to rotate
shift_pose = deepcopy(start_shake_pose)
r_angle = (r_angle / 180.0) * 3.14
pre_p_angle = (pre_p_angle / 180.0) * 3.14
shift_pose.pose.position.x -= amp * math.cos(r_angle) * math.cos(
pre_p_angle) # in verticle direction
shift_pose.pose.position.y += amp * math.cos(r_angle) * math.sin(
pre_p_angle)
shift_pose.pose.position.z += amp * math.sin(r_angle) #...
signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
return signal, end_joint_state
def setupSence(self):
r_tool_size = [0.03, 0.02, 0.18]
l_tool_size = [0.03, 0.02, 0.18]
#real scene table
table_pose = PoseStamped()
table_pose.header.frame_id = self.reference_frame
table_pose.pose.position.x = -0.052
table_pose.pose.position.y = 0.65
table_pose.pose.position.z = self.table_ground + self.table_size[
2] / 2.0
table_pose.pose.orientation.w = 1.0
self.scene.add_box(self.table_id, table_pose, self.table_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = self.arm_end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.057
l_p.pose.position.z = 0.09
l_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p,
l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = self.arm_end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.057
r_p.pose.position.z = 0.09
r_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p,
r_tool_size)
#Params: pose of bottle, grasp_radius, grasp_height, grasp_theta
def get_grasp_pose(self, pose, r, theta):
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = pose.pose.position.x + r * math.sin(theta)
g_p.pose.position.y = pose.pose.position.y - r * math.cos(theta)
g_p.pose.position.z = pose.pose.position.z
g_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
g_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
math.sin(0.5 * theta))
g_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
g_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
math.cos(0.5 * theta))
return g_p
def get_pour_pose(self, pose, h, r, theta):
p_p = PoseStamped()
p_p.header.frame_id = self.arm_end_effector_link
p_p.pose.position.x = pose.pose.position.x - r * math.cos(theta)
p_p.pose.position.y = pose.pose.position.y + r * math.sin(theta)
p_p.pose.position.z = pose.pose.position.z + h
theta *= -1
p_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
p_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
math.sin(0.5 * theta))
p_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
p_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
math.cos(0.5 * theta))
return p_p
def pour_rotate(self, initial_pose, angle_pre, angle_r, r, maxtries):
angle_pre = (angle_pre / 180.0) * 3.14
angle_r_1 = (angle_r / 180.0) * 3.14
cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
final_pose = deepcopy(initial_pose)
final_pose.pose.position.x += r * (
1 - math.cos(angle_r_1)) * math.cos(angle_pre)
final_pose.pose.position.y += r * (
1 - math.cos(angle_r_1)) * math.sin(angle_pre)
final_pose.pose.position.z += r * math.sin(angle_r_1)
#~ signal,e_j_s=self.cts_rotate_delta(cur_pose,final_pose,angle_r_pre,angle_r,maxtries,True)
signal, e_j_s = self.cts_rotate(cur_pose, final_pose, angle_r,
maxtries, True)
return signal
def p_r_trail(self, angle_pre, angle_r, r, maxtries):
angle_pre = (angle_pre / 180.0) * 3.14
angle_r_1 = (angle_r / 180.0) * 3.14
initial_pose = self.arm.get_current_pose(self.arm_end_effector_link)
final_pose = deepcopy(initial_pose)
final_pose.pose.position.x += r * (
1 - math.cos(angle_r_1)) * math.cos(angle_pre)
final_pose.pose.position.y += r * (
1 - math.cos(angle_r_1)) * math.sin(angle_pre)
final_pose.pose.position.z += r * math.sin(angle_r_1)
signal, e_j_s = self.cts_rotate(initial_pose, final_pose, angle_r,
maxtries, True)
return signal
def move_back(self, back_pose):
current_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, end_j = self.cts(current_pose, back_pose, 300, True)
return signal
def pg_g_pp(self, pose, r):
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
p_i_radian = np.arctan(abs(pose.pose.position.x /
pose.pose.position.y))
p_i_angle = p_i_radian * 180.0 / 3.14
pick_list = [
p_i_angle, 5.0, 25.0, 45.0, 65.0, 15.0, 35.0, 55.0, 75.0, 10.0,
20.0, 30.0, 40.0, 50.0, 60.0
]
for i in range(len(pick_list)):
theta = (pick_list[i] / 180.0) * 3.14
if pose.pose.position.x > 0:
theta *= -1.0
grasp_pose = self.get_grasp_pose(pose, r, theta)
#pick up
signal, e_j_s = self.cts(start_pose, grasp_pose, 300, True)
if signal == 0: continue
break
rospy.sleep(1)
self.gripperCtrl(0)
rospy.sleep(2)
#move to ori_pose
current_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e_j_s = self.cts(current_pose, start_pose, 300, True)
rospy.sleep(2)
rospy.loginfo("Grasping done.")
return start_pose, grasp_pose
def pp_ps_old(self, target_pose, pour_angle, r_pour, h_pour):
for i in range(len(pour_angle)):
maxtries = 300
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
theta = (pour_angle[i] / 180.0) * 3.14
pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta)
#move to pose
signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0,
maxtries, True)
if signal_1 == 0: continue
pre_pour_angle = pour_angle[i]
rospy.loginfo("Ready for pouring.")
return pre_pour_angle
def pp_ps(self, target_pose, pour_angle, r_pour, h_pour):
maxtries = 300
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
theta = (pour_angle / 180.0) * 3.14
pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta)
#move to pose
signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0,
maxtries, True)
return signal_1
def go_back(self, ori_pose, pre_grasp_pose):
cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e1 = self.cts(cur_pose, ori_pose, 300, True)
rospy.loginfo("back to pre_grasp pose, ready for placing bottle..")
rospy.sleep(1)
signal_1, e2 = self.cts(ori_pose, pre_grasp_pose, 300, True)
rospy.loginfo("back to grasp pose, open gripper..")
rospy.sleep(1)
self.gripperCtrl(255)
rospy.sleep(1)
signal_2, e3 = self.cts(pre_grasp_pose, ori_pose, 300, True)
rospy.loginfo("back to pre_grasp pose, ready for next kind of source.")
def rotate_back(self, joint_state):
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
q_traj.append(joint_state)
t_traj.append(t_traj[-1] + 4)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene = PlanningSceneInterface()
pub_traj = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.gripperCtrl = rospy.ServiceProxy(
"/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
self.colors = dict()
rospy.sleep(1)
self.robot = moveit_commander.robot.RobotCommander()
self.arm = MoveGroupCommander('arm')
self.arm.allow_replanning(True)
cartesian = rospy.get_param('~cartesian', True)
self.arm_end_effector_link = self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names = [[]]
self.joint_names[0] = rospy.get_param('controller_joint_names')
self.traj = trajectory_msgs.msg.JointTrajectory()
self.sub_jpc = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#scene planning
self.l_id = 'l_tool'
self.r_id = 'r_tool'
self.table_id = 'table'
self.target1_id = 'target1_object'
self.target2_id = 'target2_object'
#~ self.target3_id='target3_object'
#~ self.target4_id='target4_object'
self.f_target_id = 'receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
#~ self.scene.remove_world_object(self.target3_id)
#~ self.scene.remove_world_object(self.target4_id)
self.scene.remove_world_object(self.f_target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.table_ground = 0.13
self.table_size = [0.9, 0.6, 0.018]
self.setupSence()
target1_size = [0.035, 0.035, 0.19]
target2_size = target1_size
#~ target3_size=target1_size
#~ target4_size=target1_size
self.f_target_size = [0.2, 0.2, 0.04]
f_target_pose = PoseStamped()
pre_pour_pose = PoseStamped()
target1_pose = PoseStamped()
target2_pose = PoseStamped()
#~ target3_pose=PoseStamped()
#~ target4_pose=PoseStamped()
joint_names = [
'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
]
joint_names = rospy.get_param('controller_joint_names')
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = joint_names
#final target
l_gripper = 0.18
#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
#~ self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1)
#~ self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#target localization
msg1 = rospy.wait_for_message('/aruco_simple/pose', Pose)
rospy.sleep(1)
msg2 = rospy.wait_for_message('/aruco_simple/pose2', Pose)
rospy.sleep(1)
self.add_target(f_target_pose, self.f_target_size,
self.reference_frame, -msg2.position.y - 0.257,
-msg2.position.x + 0.81 - 0.1 - l_gripper, 0, 0, 0, 1)
self.scene.add_box(self.f_target_id, f_target_pose, self.f_target_size)
self.add_target(target1_pose, target1_size, self.reference_frame,
-msg1.position.y - 0.257,
-msg1.position.x + 0.81 - 0.065, 0, 0, 0, 1)
self.scene.add_box(self.target1_id, target1_pose, target1_size)
#~ self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1)
#~ self.scene.add_box(self.target2_id,target2_pose,target2_size)
#~ self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1)
#~ self.scene.add_box(self.target3_id,target3_pose,target3_size)
#~ self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1)
#~ self.scene.add_box(self.target4_id,target4_pose,target4_size)
#attach_pose
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
self.setColor(self.target1_id, 0.8, 0, 0, 1.0)
self.setColor(self.target2_id, 0.8, 0, 0, 1.0)
#~ self.setColor(self.target3_id,0.8,0,0,1.0)
#self.setColor(self.target4_id,0.8,0,0,1.0)
self.setColor(self.f_target_id, 0.8, 0.3, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(3)
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
j_ori_state = [
-1.2628753185272217, -0.442996621131897, -0.1326361745595932,
2.333048105239868, -0.15598002076148987, -1.2167049646377563,
3.1414425373077393
]
#signal= True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
#parameter setup
tar_num = 1
maxtries = 300
r_grasp = 0.16
#~ topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"]
#~ save_data=["carrot.txt","pepper.txt","cucumber.txt"]
#ratio_table=[0.03,0.025,0.04]
for i in range(0, tar_num):
#grasp target
rospy.loginfo("Choosing source...")
#~ dfile=open(save_data[i],"a")
if i == 0:
target_pose = target1_pose
target_id = self.target1_id
target_size = target1_size
#~ amp=0.13
#~ freq=2.75
#~ r_angle=40.0
elif i == 1:
target_pose = target2_pose
target_id = self.target2_id
target_size = target2_size
#~ amp=0.15
#~ freq=2.5
#~ r_angle=45.0
elif i == 2:
target_pose = target3_pose
target_id = self.target3_id
target_size = target3_size
amp = 0.15
freq = 2.75
r_angle = 40.0
elif i == 3:
target_pose = target4_pose
target_id = self.target4_id
target_size = target4_size
amp = 0.1
freq = 2.75
r_angle = 45.0
r_pour = 0.13
r_bottle = 0.1
h_pour = 0.1
pour_angle = [0.0, -15.0, 15.0, -30.0, 30.0, -45.0, 45.0]
#~ tip_angle=[10.0,20.0,30.0,40.0,50.0,60.0,70.0,80.0,90.0]
#~ tip_angle=[15.0,30.0,45.0,60.0,75.0,90.0]
tip_angle = [20.0, 30.0, 40.0, 50.0, 60.0, 70.00]
shake_freq = [2.0, 2.25, 2.50, 2.75]
shake_amp = [0.05, 0.075, 0.1, 0.125, 0.15]
shake_per_times = 5
shake_times_tgt = 2
rospy.loginfo("Params loaded.")
rospy.loginfo("Current Source: " + str(i + 1) + " ")
#grasp and back
ori_pose, g_pose = self.pg_g_pp(target_pose, r_grasp)
ori_joint_state = self.arm.get_current_joint_values()
#move to target position for pouring
for m in range(len(pour_angle)):
for j in range(len(tip_angle)):
for k in range(len(shake_freq)):
dfile = open(
"pepper_tipangle_" + str(tip_angle[j]) +
"_shaking_freq_" + str(shake_freq[k]) + "_1", "a")
for n in range(len(shake_amp)):
pp_ps_sgn = self.pp_ps(f_target_pose,
pour_angle[m], r_pour,
h_pour)
if pp_ps_sgn == 0:
rospy.loginfo(
"pre-Pouring angle not correct, back for replanning..."
)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
initial_pose = self.arm.get_current_pose(
self.arm_end_effector_link)
pour_signal = self.pour_rotate(
initial_pose, pour_angle[m], tip_angle[j],
r_bottle, maxtries)
if pour_signal != 1:
rospy.loginfo(
"Pouring angle not correct, back for replanning..."
)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
amp = shake_amp[n]
freq = shake_freq[k]
rospy.loginfo("shaking degree : " +
str(tip_angle[j]) +
", shaking amp : " + str(amp) +
", shaking freq : " + str(freq))
shake_times = 0
signal = True
start_joint_state = self.arm.get_current_joint_values(
)
signal, end_joint_state = self.shake_a(
pour_angle[m], tip_angle[j], amp)
while signal == 1:
#~ area_ratio= rospy.wait_for_message('/ratio_carrot',Float64)
#if (area_ratio>ratio_table[i]) or (shake_times>shake_times_tgt):
if (shake_times < shake_times_tgt):
self.shaking(start_joint_state,
end_joint_state, freq,
shake_per_times)
rospy.sleep(2)
shake_times += 1
rospy.loginfo("shaking times : " +
str(shake_times) + " .")
area_ratio = rospy.wait_for_message(
'/ratio_carrot', Float64)
if shake_times == shake_times_tgt:
dfile.write(
"%f %f \r\n" %
(shake_amp[n], area_ratio.data))
self.rotate_back(ori_joint_state)
rospy.sleep(2)
else:
signal = 0
self.rotate_back(ori_joint_state)
rospy.sleep(2)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
#remove and shut down
self.scene.remove_attached_object(self.arm_end_effector_link, 'l_tool')
self.scene.remove_attached_object(self.arm_end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)