def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None):
""" Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
if path_constraints:
if type(path_constraints) is Constraints:
constraints_str = conversions.msg_to_string(path_constraints)
else:
raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints))
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, constraints_str)
else:
(ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions)
path = RobotTrajectory()
path.deserialize(ser_path)
return (path, fraction)
def set_pose_targets(self, poses, end_effector_link=""):
""" Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
if len(end_effector_link) > 0 or self.has_end_effector_link():
if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link):
raise MoveItCommanderException("Unable to set target poses")
else:
raise MoveItCommanderException("There is no end effector to set poses for")
def place(self, object_name, location=None, plan_only=False):
"""Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided"""
result = False
if location is None:
result = self._g.place(object_name, plan_only)
elif type(location) is PoseStamped:
old = self.get_pose_reference_frame()
self.set_pose_reference_frame(location.header.frame_id)
result = self._g.place(object_name, conversions.pose_to_list(location.pose), plan_only)
self.set_pose_reference_frame(old)
elif type(location) is Pose:
result = self._g.place(object_name, conversions.pose_to_list(location), plan_only)
elif type(location) is PlaceLocation:
result = self._g.place(object_name, conversions.msg_to_string(location), plan_only)
else:
raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object")
return result
def set_pose_target(self, pose, end_effector_link=""):
""" Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
""" [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
if len(end_effector_link) > 0 or self.has_end_effector_link():
ok = False
if type(pose) is PoseStamped:
old = self.get_pose_reference_frame()
self.set_pose_reference_frame(pose.header.frame_id)
ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link)
self.set_pose_reference_frame(old)
elif type(pose) is Pose:
ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link)
else:
ok = self._g.set_pose_target(pose, end_effector_link)
if not ok:
raise MoveItCommanderException("Unable to set target pose")
else:
raise MoveItCommanderException("There is no end effector to set the pose for")
def all_close(goal, actual, tolerance):
"""
Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
@param: goal A list of floats, a Pose or a PoseStamped
@param: actual A list of floats, a Pose or a PoseStamped
@param: tolerance A float
@returns: bool
"""
all_equal = True
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index] - goal[index]) > tolerance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
return True
def set_pose_target(self, pose, end_effector_link=""):
""" Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
""" [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
if len(end_effector_link) > 0 or self.has_end_effector_link():
ok = False
if type(pose) is PoseStamped:
old = self.get_pose_reference_frame()
self.set_pose_reference_frame(pose.header.frame_id)
ok = self._g.set_pose_target(
conversions.pose_to_list(pose.pose), end_effector_link)
self.set_pose_reference_frame(old)
elif type(pose) is Pose:
ok = self._g.set_pose_target(conversions.pose_to_list(pose),
end_effector_link)
else:
ok = self._g.set_pose_target(pose, end_effector_link)
if not ok:
raise MoveItCommanderException("Unable to set target pose")
else:
raise MoveItCommanderException(
"There is no end effector to set the pose for")
def all_close(goal, actual, tolerance):
"""
Convenience method for checking if a list of values are within tolerance
goal = A list of floats, a Pose, or PoseStamped
actual = A list of floats, a Pose, or PoseStamped
tolerance = A float
returns a bool
"""
all_equal = True
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index] - goal[index]) > tolerance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
return True
def all_close(self, goal, actual, tolerance = 0.01):
"""
Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
@param: goal: A list of floats, a Pose or a PoseStamped
@param: actual: list of floats, a Pose or a PoseStamped
@param: tolerance: A float
"""
if type(goal) is list:
for index in [0, 1, 2]:
if abs(actual[index] - goal[index]) > tolerance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return self.all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
# print "pose_to_list(goal):"+str(pose_to_list(goal))
# print "pose_to_list(actual):"+str(pose_to_list(actual))
return self.all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
return True
def change_end_pose(group, input_pose):
end_goal = pose_to_list(group.get_current_pose().pose)
for i, ele in enumerate(input_pose):
if ele != None:
end_goal[i] = ele
# print "\nOUT: ", input_pose, end_goal
group.set_pose_target(list_to_pose(end_goal))
if group.go(wait=True):
# if group.plan():
print "\nSolution found for ", input_pose
print "\nGroup moved to ", end_goal
else:
print "\nNo solution found for ", input_pose
def place(self, object_name, location=None):
"""Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided"""
result = False
if location is None:
result = self._g.place(object_name)
elif type(location) is PoseStamped:
old = self.get_pose_reference_frame()
self.set_pose_reference_frame(location.header.frame_id)
result = self._g.place(object_name,
conversions.pose_to_list(location.pose))
self.set_pose_reference_frame(old)
elif type(location) is Pose:
result = self._g.place(object_name,
conversions.pose_to_list(location))
elif type(location) is PlaceLocation:
result = self._g.place(object_name,
conversions.msg_to_string(location))
else:
raise MoveItCommanderException(
"Parameter location must be a Pose, PoseStamped or PlaceLocation object"
)
return result
def set_pose_targets(self, poses, end_effector_link=""):
""" Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
if len(end_effector_link) > 0 or self.has_end_effector_link():
if not self._g.set_pose_targets(
[
conversions.pose_to_list(p) if type(p) is Pose else p
for p in poses
],
end_effector_link,
):
raise MoveItCommanderException("Unable to set target poses")
else:
raise MoveItCommanderException(
"There is no end effector to set poses for")
def all_close(self, goal, actual, tolerance):
"""
Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
@param: goal A list of floats, a Pose or a PoseStamped
@param: actual A list of floats, a Pose or a PoseStamped
@param: tolerance A float
@returns: bool
"""
try:
all_equal = True
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index] - goal[index]) > tolerance:
return False
elif type(goal) is PoseStamped:
return self.all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is Pose:
return self.all_close(pose_to_list(goal), pose_to_list(actual),
tolerance)
return True
except TypeError:
rospy.logerr(
"Incompatible types between goal and actual in 'RobotUR.allClose'"
)
def change_end_pose(group, input_pose):
pose_goal = geometry_msgs.msg.Pose()
end_goal = pose_to_list(pose_goal)
for i, ele in enumerate(input_pose):
if ele != None:
end_goal[i] = ele
group.set_pose_target(list_to_pose(end_goal))
if group.go(wait=True):
print "Solution found for ", input_pose
print "Group moved to ", end_goal
else:
print "No solution found for", input_pose
group.stop()
group.clear_pose_targets()
def pose_close(goal, actual, position_tol, orientation_tol):
"""
Convenience method for testing if a list of values are within a position_tolerance of their counterparts in another list
@param: goal A list of floats, a Pose or a PoseStamped
@param: actual A list of floats, a Pose or a PoseStamped
@param: position_tolerance A float
@returns: bool
"""
if type(goal) is list:
pos_actual = np.array(actual[0:3])
q_actual = np.array(actual[3:7])
pos_goal = np.array(goal[0:3])
q_goal = np.array(goal[3:7])
# check position
diff = np.abs(pos_goal - pos_actual)
position_close = np.all(diff < position_tol)
# https://math.stackexchange.com/a/90098
dist = 2 * np.inner(q_goal, q_actual)**2 - 1
diff = abs(np.arccos(dist))
orientation_close = np.all(diff < orientation_tol)
return position_close, orientation_close
elif type(goal) is PoseStamped:
return pose_close(goal.pose, actual.pose, position_tol,
orientation_tol)
elif type(goal) is Pose:
return pose_close(pose_to_list(goal), pose_to_list(actual),
position_tol, orientation_tol)
return True
def pose_to_lists(pose_msg, orientation_type):
"""
if orientation_type == euler, retrun euler angles in radians
"""
# print(orientation_type)
pose = pose_to_list(pose_msg)
position = pose[0:3]
quaternion = pose[3:7]
if orientation_type == "euler":
euler = euler_from_quaternion(quaternion)
orientation = euler
elif orientation_type == "quaternion":
orientation = quaternion
else:
raise MoveItCommanderException(
"Unknown type, accepts type 'euler' or 'quaternion'")
return position, orientation
def pose2affine(pose):
"""Convert pose to affine transform.
Example:
>>> p = conversions.list_to_pose([1,2,3,0,1,0,0])
>>> pose2affine(p)
array([[-1., 0., 0., 1.],
[ 0., 1., 0., 2.],
[ 0., 0., -1., 3.],
[ 0., 0., 0., 1.]])
"""
lst = conversions.pose_to_list(pose)
pos = np.array(lst[:3])
rot = np.array(lst[3:])
T = np.zeros((4, 4))
T[np.ix_([0, 1, 2], [0, 1, 2])] = q2mat(rot)
T[np.ix_([0, 1, 2], [3])] = pos.reshape(3, 1)
T[np.ix_([3], [0, 1, 2, 3])] = [0, 0, 0, 1]
return T
def move_pose_relative(self, dpose, velocity):
if velocity is None:
self.moveg.set_max_velocity_scaling_factor(self.velocity)
else:
self.moveg.set_max_velocity_scaling_factor(velocity)
pose = pose_to_list(self.moveg.get_current_pose().pose)
t_xform = np.dot(tr.translation_matrix(pose[0:3]),
tr.quaternion_matrix(pose[3:]))
s_xform = np.dot(tr.translation_matrix(dpose[0:3]),
tr.euler_matrix(*dpose[3:]))
xform = np.dot(t_xform, s_xform)
pose = tr.translation_from_matrix(xform).tolist() + list(
tr.euler_from_matrix(xform))
wp = [list_to_pose(pose)] # waypoints
self.moveg.set_start_state(self.robot.get_current_state())
(plan,
fraction) = self.moveg.compute_cartesian_path(wp,
eef_step=0.01,
jump_threshold=0.0)
if fraction < 1.0:
self.last_error_msg = "No motion plan found."
return False
v = self.velocity if velocity is None else velocity
plan = self.moveg.retime_trajectory(self.robot.get_current_state(),
plan, v)
try:
self.moveg.execute(plan, wait=True)
self.moveg.stop()
except MoveItCommanderException as e:
self.last_error_msg = str(e)
return False
return True
def all_close(goal, actual, tolerance):
"""
Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
@param: goal A list of floats, a Pose or a PoseStamped
@param: actual A list of floats, a Pose or a PoseStamped
@param: tolerance A float
@returns: bool
"""
all_equal = True
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index] - goal[index]) > tolerance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
return True
class MoveGroupPythonIntefaceTutorial(object):
"""MoveGroupPythonIntefaceTutorial"""
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move',
anonymous=True)
## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
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 Panda
## arm so we set ``group_name = panda_arm``. If you are using a different robot,
## you should change this value to the name of your robot arm planning group.
## This interface can be used to plan and execute motions on the Panda:
group_name = "manipulator"
group = moveit_commander.MoveGroupCommander(group_name)
## We create a `DisplayTrajectory`_ publisher which is used later to publish
## trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## END_SUB_TUTORIAL
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", 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 ""
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
def go_to_joint_state(self):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group
## BEGIN_SUB_TUTORIAL plan_to_joint_state
##
## Planning to a Joint Goal
## ^^^^^^^^^^^^^^^^^^^^^^^^
## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first
## thing we want to do is move it to a slightly better configuration.
# We can get the joint values from the group and adjust some of the values:
joint_goal = group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi/4
joint_goal[2] = 0
joint_goal[3] = -pi/2
joint_goal[4] = 0
joint_goal[5] = pi/3
joint_goal[6] = 0
# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
group.go(joint_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
group.stop()
## END_SUB_TUTORIAL
# For testing:
# Note that since this section of code will not be included in the tutorials
# we use the class variable rather than the copied state variable
current_joints = self.group.get_current_joint_values()
return all_close(joint_goal, current_joints, 0.01)
def go_to_pose_goal(self):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group
## BEGIN_SUB_TUTORIAL plan_to_pose
##
## Planning to a Pose Goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector:
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 0.4
pose_goal.position.y = 0.1
pose_goal.position.z = 0.4
group.set_pose_target(pose_goal)
## Now, we call the planner to compute the plan and execute it.
plan = group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
group.clear_pose_targets()
## END_SUB_TUTORIAL
# For testing:
# Note that since this section of code will not be included in the tutorials
# we use the class variable rather than the copied state variable
current_pose = self.group.get_current_pose().pose
return all_close(pose_goal, current_pose, 0.01)
def plan_cartesian_path(self, scale=1):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group
## BEGIN_SUB_TUTORIAL plan_cartesian_path
##
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a Cartesian path directly by specifying a list of waypoints
## for the end-effector to go through:
##
waypoints = []
wpose = group.get_current_pose().pose
wpose.position.z -= scale * 0.1 # First move up (z)
wpose.position.y += scale * 0.2 # and sideways (y)
waypoints.append(copy.deepcopy(wpose))
wpose.position.x += scale * 0.1 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
wpose.position.y -= scale * 0.1 # Third move sideways (y)
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 disable the jump threshold by setting it to 0.0 disabling:
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
# Note: We are just planning, not asking move_group to actually move the robot yet:
return plan, fraction
## END_SUB_TUTORIAL
def display_trajectory(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
robot = self.robot
display_trajectory_publisher = self.display_trajectory_publisher
## BEGIN_SUB_TUTORIAL display_trajectory
##
## Displaying a Trajectory
## ^^^^^^^^^^^^^^^^^^^^^^^
## You can ask RViz to visualize a plan (aka trajectory) for you. But the
## group.plan() method does this automatically so this is not that useful
## here (it just displays the same trajectory again):
##
## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory.
## We populate the trajectory_start with our current robot state to copy over
## any AttachedCollisionObjects and add our plan to the trajectory.
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory);
## END_SUB_TUTORIAL
def execute_plan(self, plan):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
group = self.group
## BEGIN_SUB_TUTORIAL execute_plan
##
## Executing a Plan
## ^^^^^^^^^^^^^^^^
## Use execute if you would like the robot to follow
## the plan that has already been computed:
group.execute(plan, wait=True)
## **Note:** The robot's current joint state must be within some tolerance of the
## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail
## END_SUB_TUTORIAL
def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
## BEGIN_SUB_TUTORIAL wait_for_scene_update
##
## Ensuring Collision Updates Are Receieved
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## If the Python node dies before publishing a collision object update message, the message
## could get lost and the box will not appear. To ensure that the updates are
## made, we wait until we see the changes reflected in the
## ``get_known_object_names()`` and ``get_known_object_names()`` lists.
## For the purpose of this tutorial, we call this function after adding,
## removing, attaching or detaching an object in the planning scene. We then wait
## until the updates have been made or ``timeout`` seconds have passed
start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
# Test if the box is in attached objects
attached_objects = scene.get_attached_objects([box_name])
is_attached = len(attached_objects.keys()) > 0
# Test if the box is in the scene.
# Note that attaching the box will remove it from known_objects
is_known = box_name in scene.get_known_object_names()
# Test if we are in the expected state
if (box_is_attached == is_attached) and (box_is_known == is_known):
return True
# Sleep so that we give other threads time on the processor
rospy.sleep(0.1)
seconds = rospy.get_time()
# If we exited the while loop without returning then we timed out
return False
## END_SUB_TUTORIAL
def add_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
## BEGIN_SUB_TUTORIAL add_box
##
## Adding Objects to the Planning Scene
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will create a box in the planning scene at the location of the left finger:
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_leftfinger"
box_pose.pose.orientation.w = 1.0
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))
## END_SUB_TUTORIAL
# Copy local variables back to class variables. In practice, you should use the class
# variables directly unless you have a good reason not to.
self.box_name=box_name
return self.wait_for_state_update(box_is_known=True, timeout=timeout)
def attach_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
robot = self.robot
scene = self.scene
eef_link = self.eef_link
group_names = self.group_names
## BEGIN_SUB_TUTORIAL attach_object
##
## Attaching Objects to the Robot
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## Next, we will attach the box to the Panda wrist. Manipulating objects requires the
## robot be able to touch them without the planning scene reporting the contact as a
## collision. By adding link names to the ``touch_links`` array, we are telling the
## planning scene to ignore collisions between those links and the box. For the Panda
## robot, we set ``grasping_group = 'hand'``. If you are using a different robot,
## you should change this value to the name of your end effector group name.
grasping_group = 'hand'
touch_links = robot.get_link_names(group=grasping_group)
scene.attach_box(eef_link, box_name, touch_links=touch_links)
## END_SUB_TUTORIAL
# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=timeout)
def detach_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
eef_link = self.eef_link
## BEGIN_SUB_TUTORIAL detach_object
##
## Detaching Objects from the Robot
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## We can also detach and remove the object from the planning scene:
scene.remove_attached_object(eef_link, name=box_name)
## END_SUB_TUTORIAL
# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_known=True, box_is_attached=False, timeout=timeout)
def remove_box(self, timeout=4):
# Copy class variables to local variables to make the web tutorials more clear.
# In practice, you should use the class variables directly unless you have a good
# reason not to.
box_name = self.box_name
scene = self.scene
## BEGIN_SUB_TUTORIAL remove_object
##
## Removing Objects from the Planning Scene
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## We can remove the box from the world.
scene.remove_world_object(box_name)
## **Note:** The object must be detached before we can remove it from the world
## END_SUB_TUTORIAL
# We wait for the planning scene to update.
return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout)
def main():
try:
print "============ Press `Enter` to begin the tutorial by setting up the moveit_commander (press ctrl-d to exit) ..."
raw_input()
tutorial = MoveGroupPythonIntefaceTutorial()
print "============ Press `Enter` to execute a movement using a joint state goal ..."
raw_input()
tutorial.go_to_joint_state()
print "============ Press `Enter` to execute a movement using a pose goal ..."
raw_input()
tutorial.go_to_pose_goal()
print "============ Press `Enter` to plan and display a Cartesian path ..."
raw_input()
cartesian_plan, fraction = tutorial.plan_cartesian_path()
print "============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ..."
raw_input()
tutorial.display_trajectory(cartesian_plan)
print "============ Press `Enter` to execute a saved path ..."
raw_input()
tutorial.execute_plan(cartesian_plan)
print "============ Press `Enter` to add a box to the planning scene ..."
raw_input()
tutorial.add_box()
print "============ Press `Enter` to attach a Box to the Panda robot ..."
raw_input()
tutorial.attach_box()
print "============ Press `Enter` to plan and execute a path with an attached collision object ..."
raw_input()
cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1)
tutorial.execute_plan(cartesian_plan)
print "============ Press `Enter` to detach the box from the Panda robot ..."
raw_input()
tutorial.detach_box()
print "============ Press `Enter` to remove the box from the planning scene ..."
raw_input()
tutorial.remove_box()
print "============ Python tutorial demo complete!"
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
if __name__ == '__main__':
main()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('forward_kinematics', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_arm = moveit_commander.MoveGroupCommander("arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
planning_frame = group_arm.get_planning_frame()
print "============ \nReference frame: %s" % planning_frame
eef_link = group_arm.get_end_effector_link()
print "End effector: %s" % eef_link
group_names = robot.get_group_names()
print "Robot Groups:", robot.get_group_names(), "\n============"
print "Group_arm pose\n", group_arm.get_current_pose()
print "Joint Angles\n", group_arm.get_current_joint_values()
# change_joint_angles(group_arm, [1.2, 0.4, -1.57, 1, 0.3])
# change_joint_angles(group_arm, [-3.0357, 0, 0, 0, 0])
# change_joint_angles(group_arm, [3.24426, 0, 0, 0, 0])
change_joint_angles(group_arm, [0, 0, -1.57, -1.57, -2.47])
# change_joint_angles(group_arm, [0, -1.973232422, -1.99, -2.36344086, -2.616666666])
# change_joint_angles(group_arm, [0, 0.57, 0, 0, 0])
#[0.0, -0.2477628272874607, 0.4472224202239656, 0.3047862245403626, 0.07725225351232594, -0.23323341117218666, 0.9201845589721035]
print "\nGroup_arm pose\n", group_arm.get_current_pose()
print "Joint Angles\n", group_arm.get_current_joint_values()
print "\nGroup_arm pose\n", pose_to_list(group_arm.get_current_pose().pose)
def all_close(goal, actual, tolerance):
x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual)
x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal)
d = dist((x1, y1, z1), (x0, y0, z0))
cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1)
return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0)