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 on_enter(self, userdata):
self._failed = False
request = ExecuteKnownTrajectoryRequest()
request.trajectory.joint_trajectory = userdata.joint_trajectory
request.wait_for_execution = True
try:
self._result = self._srv.call(self._topic, request)
except Exception as e:
Logger.logwarn("Was unable to execute joint trajectory:\n%s" % str(e))
self._failed = True
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 go_to_pose(self,
joint_names,
positions,
time=3.0,
wait_for_execution=True):
req = ExecuteKnownTrajectoryRequest()
req.trajectory.joint_trajectory.joint_names = joint_names
p = JointTrajectoryPoint()
p.positions = positions
p.time_from_start = rospy.Duration(time)
req.trajectory.joint_trajectory.points.append(p)
req.wait_for_execution = wait_for_execution
try:
resp = self.ekt_srv.call(req)
return resp
except rospy.ServiceException as e:
rospy.logerr("Service exception: " + str(e))
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 go_to_configuration(self,
joint_names,
positions,
time=None,
wait_for_execution=None):
"""
Execute a trajectory from current configuration to
the given configuration.
:param list of str joint_names: List of joint names.
:param list of float positions: List of positions.
"""
if len(joint_names) != len(positions):
rospy.logerr("joint_names and positions have different lengths!")
resp = ExecuteKnownTrajectoryResponse()
resp.error_code = 9999 # Failure
return resp
if time is None:
time = self.time
if wait_for_execution is None:
wait_for_execution = self.wait_for_execution
req = ExecuteKnownTrajectoryRequest()
req.trajectory.joint_trajectory.joint_names = joint_names
p = JointTrajectoryPoint()
p.positions = positions
p.time_from_start = rospy.Duration(time)
req.trajectory.joint_trajectory.points.append(p)
req.wait_for_execution = wait_for_execution
try:
resp = self.ekt_srv.call(req)
return resp
except rospy.ServiceException as e:
rospy.logerr("Service exception: " + str(e))
resp = ExecuteKnownTrajectoryResponse()
resp.error_code = 9999 # Failure
return resp
def on_enter(self, userdata):
self._param_error = False
self._request_failed = False
self._moveit_failed = False
self._success = False
# Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn(
'Unable to parse given SRDF parameter: /robot_description_semantic'
)
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib[
'name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' %
self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib[
'group'] #Set move group name in case it was not defined
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' %
self._config_name)
return 'param_error'
try:
self._joint_config = [
float(j.attrib['value']) for j in config.iter('joint')
]
self._joint_names = [
str(j.attrib['name']) for j in config.iter('joint')
]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' %
str(e))
return 'param_error'
# Action Initialization
action_goal = ExecuteKnownTrajectoryRequest(
) # ExecuteTrajectoryGoal()
#action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.3)
action_goal.trajectory.joint_trajectory.joint_names = self._joint_names
action_goal.trajectory.joint_trajectory.points = [
JointTrajectoryPoint()
]
action_goal.trajectory.joint_trajectory.points[
0].time_from_start = rospy.Duration(self._duration)
action_goal.wait_for_execution = self._wait_for_execution
action_goal.trajectory.joint_trajectory.points[
0].positions = self._joint_config
try:
self._response = self._client.call(self._action_topic,
action_goal)
Logger.loginfo(
"Execute Known Trajectory Service requested: \n" +
str(self._action_topic))
except rospy.ServiceException as exc:
Logger.logwarn(
"Execute Known Trajectory Service did not process request: \n"
+ str(exc))
self._request_failed = True
return
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"
def on_enter(self, userdata):
self._param_error = False
self._request_failed = False
self._moveit_failed = False
self._success = False
# Parameter check
if self._srdf_param is None:
self._param_error = True
return
try:
self._srdf = ET.fromstring(self._srdf_param)
except Exception as e:
Logger.logwarn('Unable to parse given SRDF parameter: /robot_description_semantic')
self._param_error = True
if not self._param_error:
robot = None
for r in self._srdf.iter('robot'):
if self._robot_name == '' or self._robot_name == r.attrib['name']:
robot = r
break
if robot is None:
Logger.logwarn('Did not find robot name in SRDF: %s' % self._robot_name)
return 'param_error'
config = None
for c in robot.iter('group_state'):
if (self._move_group == '' or self._move_group == c.attrib['group']) \
and c.attrib['name'] == self._config_name:
config = c
self._move_group = c.attrib['group'] #Set move group name in case it was not defined
break
if config is None:
Logger.logwarn('Did not find config name in SRDF: %s' % self._config_name)
return 'param_error'
try:
self._joint_config = [float(j.attrib['value']) for j in config.iter('joint')]
self._joint_names = [str(j.attrib['name']) for j in config.iter('joint')]
except Exception as e:
Logger.logwarn('Unable to parse joint values from SRDF:\n%s' % str(e))
return 'param_error'
# Action Initialization
action_goal = ExecuteKnownTrajectoryRequest() # ExecuteTrajectoryGoal()
#action_goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.3)
action_goal.trajectory.joint_trajectory.joint_names = self._joint_names
action_goal.trajectory.joint_trajectory.points = [JointTrajectoryPoint()]
action_goal.trajectory.joint_trajectory.points[0].time_from_start = rospy.Duration(self._duration)
action_goal.wait_for_execution = self._wait_for_execution
action_goal.trajectory.joint_trajectory.points[0].positions = self._joint_config
try:
self._response = self._client.call(self._action_topic, action_goal)
Logger.loginfo("Execute Known Trajectory Service requested: \n" + str(self._action_topic))
except rospy.ServiceException as exc:
Logger.logwarn("Execute Known Trajectory Service did not process request: \n" + str(exc))
self._request_failed = True
return