def createExecuteKnownTrajectoryRequest(trajectory, wait_for_execution=True):
"""Create a ExecuteKnownTrajectoryRequest from the given data,
trajectory must be a RobotTrajectory probably filled from a GetCartesianPath call"""
ektr = ExecuteKnownTrajectoryRequest()
ektr.trajectory = trajectory
ektr.wait_for_execution = wait_for_execution
return ektr
def createExecuteKnownTrajectoryRequest(trajectory, wait_for_execution=True):
"""Create a ExecuteKnownTrajectoryRequest from the given data,
trajectory must be a RobotTrajectory probably filled from a GetCartesianPath call"""
ektr = ExecuteKnownTrajectoryRequest()
ektr.trajectory = trajectory
ektr.wait_for_execution = wait_for_execution
return ektr
def execute_trajectory(trajectory, wait_for_execution=True):
req = ExecuteKnownTrajectoryRequest()
req.trajectory = RobotTrajectory()
req.trajectory.joint_trajectory = trajectory
req.wait_for_execution = wait_for_execution
srv = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
res = srv(req)
if wait_for_execution:
rospy.sleep(1)
return res.error_code
def execute(trajectory, wait=True, persistent=False, wait_timeout=None):
"""Call set robot speed service
INPUT
trajectory [RobotTrajectory]
wait [Bool]
persistent [Bool, default=False]
wait_timeout [Float, default=None]
OUTPUT
response [SetSpeedResponse]
"""
srv_proxy = get_service_proxy(SRV_EXECUTE, ExecuteKnownTrajectory,
persistent, wait_timeout)
req = ExecuteKnownTrajectoryRequest()
req.trajectory = trajectory
req.wait_for_execution = wait
res = srv_proxy(req)
return res
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("right_arm")
#group.set_planner_id("PRMkConfigDefault")
## 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)
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
rospy.sleep(5)
print "Going to compute a cartesian path from where we are to z + 0.3"
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a cartesian path directly by specifying a list of waypoints
## for the end-effector to go through.
waypoints = []
# start with the current pose
rospy.sleep(1)
waypoints.append(group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z + 0.15
waypoints.append(copy.deepcopy(wpose))
# fourth move to the side a lot
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
fraction = 0.0
jump_threshold = 0
eef_step = 0.01
while fraction < 0.9:
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
eef_step, # eef_step
jump_threshold) # jump_threshold
print "Fraction is: " + str(fraction) + " if it's less than 0.9 we will continue calculating"
print "changing jump_threshold + 10 it was: " + str(jump_threshold)
print "(Seems to make no difference)"
jump_threshold += 10
print "eef-step is: " + str(eef_step) + " adding 1cm"
eef_step += 0.01
print "plan looks like: " + str(plan3)
print "with fraction being: " + str(fraction)
print "Fraction should be 1, less than that, is that we went out of the straight line, i think"
print "Moving arm to cartesian path thing"
#group.go(wait=True)
rospy.sleep(1)
print "this did for sure not work, so lets try with executeknowntrajectory thing"
ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
ekp.wait_for_service()
print "!!!! Gonna send goal to execute_kinematic_path (waiting 3s)"
rospy.sleep(3)
ektr = ExecuteKnownTrajectoryRequest()
ektr.trajectory = plan3
ektr.wait_for_execution = True
print "Sending call"
ekp.call(ektr)
print "!!!! Call done"
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("right_arm")
#group.set_planner_id("PRMkConfigDefault")
## 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)
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "============ Generating plan to initial pose"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.3
pose_target.position.y = -0.3
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
plan1 = group.plan()
# Uncomment below line when working with a real robot
print "Moving arm to initial 3d pose: " + str(pose_target)
group.go(wait=True)
rospy.sleep(1)
group.clear_pose_targets()
print "============ Generating plan to final pose"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.3
pose_target.position.y = -0.3
pose_target.position.z = 1.4
group.set_pose_target(pose_target)
plan1 = group.plan()
# Uncomment below line when working with a real robot
print "Moving arm to final pose: " + str(pose_target)
group.go(wait=True)
rospy.sleep(1)
group.clear_pose_targets()
print "If everything worked well here, then we should be able to do a straight path"
print "============ Generating plan to initial pose"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.3
pose_target.position.y = -0.3
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
plan1 = group.plan()
# Uncomment below line when working with a real robot
print "Moving arm to initial 3d pose: " + str(pose_target)
group.go(wait=True)
rospy.sleep(1)
group.clear_pose_targets()
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a cartesian path directly by specifying a list of waypoints
## for the end-effector to go through.
waypoints = []
# start with the current pose
rospy.sleep(1)
waypoints.append(group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z + 0.15
waypoints.append(copy.deepcopy(wpose))
# fourth move to the side a lot
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.02, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(3)
print "plan3 looks like: " + str(plan3)
print "with fraction being: " + str(fraction)
print "Moving arm to cartesian path thing"
group.go(wait=True)
print "this did for sure not work, so lets try with executeknowntrajectory thing"
ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
ekp.wait_for_service()
print "!!!! Gonna send goal to execute_kinematic_path"
rospy.sleep(4)
ektr = ExecuteKnownTrajectoryRequest()
ektr.trajectory = plan3
ektr.wait_for_execution = True
ekp.call(ektr)
print "!!!! Call done"
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
right_group = moveit_commander.MoveGroupCommander("right_arm")
left_group = moveit_commander.MoveGroupCommander("left_arm")
#group.set_planner_id("PRMkConfigDefault")
print "Going to compute a cartesian path for right arm from where we are to z + 0.3"
### Right part
waypoints = []
# start with the current pose
rospy.sleep(1)
waypoints.append(right_group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z + 0.15
waypoints.append(copy.deepcopy(wpose))
# fourth move to the side a lot
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
(planright, fraction) = right_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.02, # eef_step
0.0) # jump_threshold
print "planright looks like: " + str(planright)
print "with fraction being: " + str(fraction)
#### left part
waypoints = []
# start with the current pose
rospy.sleep(1)
waypoints.append(left_group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z + 0.15
waypoints.append(copy.deepcopy(wpose))
# fourth move to the side a lot
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
(planleft, fraction) = left_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.02, # eef_step
0.0) # jump_threshold
print "planleft looks like: " + str(planleft)
print "with fraction being: " + str(fraction)
### EXECUTE STUFF
rospy.sleep(1)
finalplan = join_trajectories(planright, planleft)
print "final plan looks like: " + str(finalplan)
print "so lets try with executeknowntrajectory thing"
ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
ekp.wait_for_service()
print "!!!! Gonna send BOTH ARMS goal to execute_kinematic_path"
#rospy.sleep(3)
ektr = ExecuteKnownTrajectoryRequest()
ektr.trajectory = finalplan
ektr.wait_for_execution = True
print "Sending call both arms"
ekp.call(ektr)
print "!!!! Call done"
#
# print "!!!! Gonna send LEFT goal to execute_kinematic_path"
# #rospy.sleep(3)
# ektr = ExecuteKnownTrajectoryRequest()
# ektr.trajectory = planleft
# ektr.wait_for_execution = True
# print "Sending call left arm"
# ekp.call(ektr)
# print "!!!! Call done"
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
right_group = moveit_commander.MoveGroupCommander("right_arm")
left_group = moveit_commander.MoveGroupCommander("left_arm")
#group.set_planner_id("PRMkConfigDefault")
print "Going to compute a cartesian path for right arm from where we are to z + 0.3"
### Right part
waypoints = []
# start with the current pose
rospy.sleep(1)
waypoints.append(right_group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z + 0.15
waypoints.append(copy.deepcopy(wpose))
# fourth move to the side a lot
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
(planright, fraction) =right_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.02, # eef_step
0.0) # jump_threshold
print "planright looks like: " + str(planright)
print "with fraction being: " + str(fraction)
#### left part
waypoints = []
# start with the current pose
rospy.sleep(1)
waypoints.append(left_group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z + 0.15
waypoints.append(copy.deepcopy(wpose))
# fourth move to the side a lot
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
(planleft, fraction) = left_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.02, # eef_step
0.0) # jump_threshold
print "planleft looks like: " + str(planleft)
print "with fraction being: " + str(fraction)
### EXECUTE STUFF
rospy.sleep(1)
finalplan = join_trajectories(planright, planleft)
print "final plan looks like: " + str(finalplan)
print "so lets try with executeknowntrajectory thing"
ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
ekp.wait_for_service()
print "!!!! Gonna send BOTH ARMS goal to execute_kinematic_path"
#rospy.sleep(3)
ektr = ExecuteKnownTrajectoryRequest()
ektr.trajectory = finalplan
ektr.wait_for_execution = True
print "Sending call both arms"
ekp.call(ektr)
print "!!!! Call done"
#
# print "!!!! Gonna send LEFT goal to execute_kinematic_path"
# #rospy.sleep(3)
# ektr = ExecuteKnownTrajectoryRequest()
# ektr.trajectory = planleft
# ektr.wait_for_execution = True
# print "Sending call left arm"
# ekp.call(ektr)
# print "!!!! Call done"
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("right_arm")
#group.set_planner_id("PRMkConfigDefault")
## 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)
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "============ Generating plan to initial pose"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.3
pose_target.position.y = -0.3
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
plan1 = group.plan()
# Uncomment below line when working with a real robot
print "Moving arm to initial 3d pose: " + str(pose_target)
group.go(wait=True)
rospy.sleep(1)
group.clear_pose_targets()
print "============ Generating plan to final pose"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.3
pose_target.position.y = -0.3
pose_target.position.z = 1.4
group.set_pose_target(pose_target)
plan1 = group.plan()
# Uncomment below line when working with a real robot
print "Moving arm to final pose: " + str(pose_target)
group.go(wait=True)
rospy.sleep(1)
group.clear_pose_targets()
print "If everything worked well here, then we should be able to do a straight path"
print "============ Generating plan to initial pose"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.3
pose_target.position.y = -0.3
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
plan1 = group.plan()
# Uncomment below line when working with a real robot
print "Moving arm to initial 3d pose: " + str(pose_target)
group.go(wait=True)
rospy.sleep(1)
group.clear_pose_targets()
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a cartesian path directly by specifying a list of waypoints
## for the end-effector to go through.
waypoints = []
# start with the current pose
rospy.sleep(1)
waypoints.append(group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z + 0.15
waypoints.append(copy.deepcopy(wpose))
# fourth move to the side a lot
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.02, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(3)
print "plan3 looks like: " + str(plan3)
print "with fraction being: " + str(fraction)
print "Moving arm to cartesian path thing"
group.go(wait=True)
print "this did for sure not work, so lets try with executeknowntrajectory thing"
ekp = rospy.ServiceProxy('/execute_kinematic_path', ExecuteKnownTrajectory)
ekp.wait_for_service()
print "!!!! Gonna send goal to execute_kinematic_path"
rospy.sleep(4)
ektr = ExecuteKnownTrajectoryRequest()
ektr.trajectory = plan3
ektr.wait_for_execution = True
ekp.call(ektr)
print "!!!! Call done"
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"