def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = 0.5
request.ik_request.pose_stamped.pose.position.y = 0.5
request.ik_request.pose_stamped.pose.position.z = 0.0
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("right_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_hand"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = 0.5
request.ik_request.pose_stamped.pose.position.y = 0.5
request.ik_request.pose_stamped.pose.position.z = 0.3
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("left_arm")
#this command tries to achieve a pose: which is position, orientation
group.set_pose_target(request.ik_request.pose_stamped)
#then, this command tries to achieve a position only. orientation is airbitrary.
group.set_position_target([0.5,0.5,0.3])
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_position', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
arm.set_planning_time(10)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
print "======== create new goal position ========"
start_pose = PoseStamped()
start_pose.header.frame_id = arm.get_planning_frame()
# Test Position
start_pose.pose.position.x = 0.2 # 20 cm
start_pose.pose.position.y = -0.11 # -11 cm
start_pose.pose.position.z = 0.6 # 60 cm
q = quaternion_from_euler(0.0, 0.0, 0.0)
start_pose.pose.orientation = Quaternion(*q)
print start_pose
print "====== move to position ======="
# KDL
# arm.set_joint_value_target(start_pose, True)
# IK Fast
arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z],
arm.get_end_effector_link())
arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class SrRobotCommander(object):
"""
Base class for hand and arm commanders
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy(
"get_robot_state", GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {
item["name"]: item["joints"]
for item in controller_list_param
}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def set_planner_id(self, planner_id):
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(
num_planning_attempts)
def set_planning_time(self, seconds):
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
"""
if self.check_plan_is_valid():
self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
def execute_plan(self, plan):
if self.check_given_plan_is_valid(plan):
self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
def move_to_joint_value_target(self,
joint_states,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not
This is a blocking method.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None
and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
self._move_group_commander.set_joint_value_target(js)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander.\
_g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return geometry_msgs.msg.Pose() - current pose of the end effector
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(
reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0), rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn(
"Couldn't get the pose from " +
self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return a dictionary with the joint names as keys and current joint values
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return a dictionary with the joint names as keys and current joint values
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position
@return - dictionary with joints positions
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = current.values()
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(
wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr(
"Invalid waypoint. Must contain valid name for named target or dict of joint angles."
)
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[
n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [
new_positions[n] for n in joint_names
]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = current.values()
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = current.keys()
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link=""):
"""
Specify a target position for the end-effector and plans.
This is a blocking method.
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self.__plan = self._move_group_commander.plan()
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: 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]
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self,
pose,
end_effector_link="",
alternative_method=False):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: 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]
@param end_effector_link - name of the end effector link
@param alternative_method - use set_joint_value_target instead of set_pose_target
"""
self._move_group_commander.set_start_state_to_current_state()
if alternative_method:
self._move_group_commander.set_joint_value_target(
pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {
n: p
for n, p in zip(joint_state.name, joint_state.position)
}
self._joints_velocity = {
n: v
for n, v in zip(joint_state.name, joint_state.velocity)
}
self._joints_effort = {
n: v
for n, v in zip(joint_state.name, joint_state.effort)
}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(
service_name, FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(
timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self,
joint_states,
time=0.002,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller)
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client],
lambda terminal_state, result: self._action_done_cb(
client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i]
for i in indices_of_joints_in_this_controller
]
for point in goal.trajectory.points:
if point.positions:
point.positions = [
point.positions[i]
for i in indices_of_joints_in_this_controller
]
if point.velocities:
point.velocities = [
point.velocities[i]
for i in indices_of_joints_in_this_controller
]
if point.effort:
point.effort = [
point.effort[i]
for i in indices_of_joints_in_this_controller
]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self,
waypoints,
reference_frame=None,
eef_step=0.005,
jump_threshold=0.0):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given
@param waypoints - an array of poses of end-effector
@param eef_step - configurations are computed for every eef_step meters
@param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
"""
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
(self.__plan,
fraction) = self._move_group_commander.compute_cartesian_path(
waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory
@param trajectory - moveit_msgs/JointTrajectory
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
)
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.5
service_request.avoid_collisions = avoid_collisions
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" %
resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
pose_target_2 = Pose()
pose_target_2.position.x = 0.3
pose_target_2.position.y = -0.5
pose_target_2.position.z = 0.15
pose_target_2.orientation.x = 0.0
pose_target_2.orientation.y = -0.707
pose_target_2.orientation.z = 0.0
pose_target_2.orientation.w = 0.707
group.set_planner_id("RRTConnectkConfigDefault")
group.allow_replanning(True)
rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_2))
xyz = [
pose_target_2.position.x, pose_target_2.position.y,
pose_target_2.position.z
]
group.set_position_target(xyz)
result_p = group.go()
rospy.loginfo("Moving to the Position Executed... {}".format(result_p))
group.clear_path_constraints()
if result_p:
group.set_pose_target(pose_target_2)
result_o = group.go()
rospy.loginfo(
"Adjusting the Orientation Executed... {}".format(result_o))
# 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")
rospy.sleep(1)
# take the part to specific pose
pose = generate_rand_position()
# IKFast
arm.set_position_target([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z],
arm.get_end_effector_link())
# KDL
# arm.set_joint_value_target(pose, True)
arm.go()
rospy.sleep(5)
# ===== place start ==== #
place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())
rospy.loginfo("Place Result is:")
rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val]))
rospy.sleep(5)
# end
moveit_commander.roscpp_shutdown()
#!/usr/bin/env python
import rospy
from moveit_commander import MoveGroupCommander, RobotCommander, PlanningSceneInterface
rospy.init_node("goto_point")
robot = RobotCommander()
gripper_group = MoveGroupCommander("gripper_group")
scene = PlanningSceneInterface()
rospy.sleep(2)
xyz_goal = [0, 0.0, 1.5]
gripper_group.set_position_target(xyz_goal,
gripper_group.get_end_effector_link())
gripper_group.go()
gripper_target_1 = [ 0.01, 0]
gripper_group.set_joint_value_target(gripper_target_1)
gripper_group.go()
rospy.sleep(5.0)
pose_current = group.get_current_pose()
rospy.loginfo( "Get Current Pose:\n{}\n".format( pose_current ) )
# Pose 2
rospy.loginfo( "Starting Pose 2")
group.set_position_target([0.2, -0.1, 0.05])
group.go()
gripper_target_2 = [ -0.01, 0]
gripper_group.set_joint_value_target(gripper_target_2)
gripper_group.go()
rospy.sleep(5.0)
pose_current = group.get_current_pose()
rospy.loginfo( "Get Current Pose:\n{}\n".format( pose_current ) )
# Pose 3
rospy.loginfo( "Starting Pose 3")
pose_target_3 = Pose()
# 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")
rospy.sleep(1)
# take the part to specific pose
pose = generate_rand_position()
# IKFast
arm.set_position_target(
[pose.pose.position.x, pose.pose.position.y, pose.pose.position.z],
arm.get_end_effector_link())
# KDL
# arm.set_joint_value_target(pose, True)
arm.go()
rospy.sleep(5)
# ===== place start ==== #
place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())
rospy.loginfo("Place Result is:")
rospy.loginfo("Human readable error: " +
str(moveit_error_dict[place_result.error_code.val]))
rospy.sleep(5)
class PathPlanner:
"""
This path planner wraps the planning and actuation functionality provided by MoveIt.
All positions are arrays holding x, y, and z coordinates. Orientations are
arrays holding x, y, z, and w coordinates (in quaternion form).
Attributes:
frame_id (str): The frame all coordinates are relative to.
workspace (list): The bounds of the planning space (a box). Specified
as the minimum x, y, and z coordinates, then the maximum x, y, and
z coordinates.
group_name (str): The MoveIt group name (for example, 'right_arm').
time_limit (float): The maximum number of seconds MoveIt will plan for.
robot: The MoveIt robot commander.
scene: The planning scene.
group: The MoveIt MoveGroup.
scene_publisher: A publisher that updates the planning scene.
"""
PLANNING_SCENE_TOPIC = '/collision_object'
def __init__(self, frame_id, group_name, time_limit=10, workspace=None,
register_shutdown=True):
if workspace is None:
workspace = [-2, -2, -2, 2, 2, 2]
if rospy.get_param('verbose'):
rospy.loginfo('Move group: {}'.format(group_name))
self.frame_id, self.workspace = frame_id, workspace
self.time_limit, self.group_name = time_limit, group_name
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
self.scene_publisher = rospy.Publisher(self.PLANNING_SCENE_TOPIC,
CollisionObject, queue_size=10)
self.group = MoveGroupCommander(group_name)
if register_shutdown:
rospy.on_shutdown(self.shutdown)
self.group.set_planning_time(time_limit)
self.group.set_workspace(workspace)
rospy.sleep(0.5) # Sleep to ensure initialization has finished.
if rospy.get_param('verbose'):
rospy.loginfo('Initialized path planner.')
def shutdown(self):
""" Stop the path planner safely. """
self.group.stop()
del self.group
if rospy.get_param('verbose'):
rospy.logwarn('Terminated path planner.')
def move_to_pose(self, position, orientation=None):
"""
Move the end effector to the given pose naively.
Arguments:
position: The x, y, and z coordinates to move to.
orientation: The orientation to take (quaternion, optional).
Raises (rospy.ServiceException): Failed to execute movement.
"""
if orientation is None:
self.group.set_position_target(position)
else:
self.group.set_pose_target(create_pose(self.frame_id, position, orientation))
if rospy.get_param('verbose'):
log_pose('Moving to pose.', position, orientation)
self.group.go(wait=True)
self.group.stop()
self.group.clear_pose_targets()
def move_to_pose_with_planner(self, position, orientation=None, orientation_constraints=None):
"""
Move the end effector to the given pose, taking into account
constraints and planning scene obstacles.
Arguments:
position: The x, y, and z coordinates to move to.
orientation: The orientation to take (quaternion, optional).
orientation_constraints (list): A list of `OrientationConstraint`
objects created with `make_orientation_constraint`.
Returns (bool): Whether or not the movement executed successfully.
"""
if orientation_constraints is None:
orientation_constraints = []
target = create_pose(self.frame_id, position, orientation)
if rospy.get_param('verbose'):
log_pose('Moving to pose with planner.', position, orientation)
for _ in range(3):
try:
plan = self.plan_to_pose(target, orientation_constraints)
if not self.group.execute(plan, True):
raise ValueError('Execution failed.')
except ValueError as exc:
rospy.logwarn('Failed to perform movement: {}. Retrying.'.format(str(exc)))
else:
break
else:
raise ValueError('Failed to move to pose.')
def plan_to_pose(self, target, orientation_constraints):
"""
Plan a movement to a pose from the current state, given constraints.
Arguments:
target: The destination pose.
orientation_constraints (list): The constraints.
Returns (moveit_msgs.msg.RobotTrajectory): The path.
"""
self.group.set_pose_target(target)
self.group.set_start_state_to_current_state()
constraints = Constraints()
constraints.orientation_constraints = orientation_constraints
self.group.set_path_constraints(constraints)
return self.group.plan()
def add_box_obstacle(self, dimensions, name, com_position, com_orientation):
"""
Add a rectangular prism obstacle to the planning scene.
Arguments:
dimensions: An array containing the width, length, and height of
the box (in the box's body frame, corresponding to x, y, and z).
name: A unique name for identifying this obstacle.
com_position: The position of the center-of-mass (COM) of this box,
relative to the global frame `frame_id`.
com_orientation: The orientation of the COM.
"""
pose = create_pose(self.frame_id, com_position, com_orientation)
obj = CollisionObject()
obj.id, obj.operation, obj.header = name, CollisionObject.ADD, pose.header
box = SolidPrimitive()
box.type, box.dimensions = SolidPrimitive.BOX, dimensions
obj.primitives, obj.primitive_poses = [box], [pose.pose]
self.scene_publisher.publish(obj)
if rospy.get_param('verbose'):
rospy.loginfo('Added box object "{}" to planning scene: '
'(x={}, y={}, z={}).'.format(name, *dimensions))
def remove_obstacle(self, name):
""" Remove a named obstacle from the planning scene. """
obj = CollisionObject()
obj.id, obj.operation = name, CollisionObject.REMOVE
self.scene_publisher.publish(obj)
if rospy.get_param('verbose'):
rospy.loginfo('Removed object "{}" from planning scene.'.format(name))
def make_orientation_constraint(self, orientation, link_id, tolerance=0.1, weight=1):
"""
Make an orientation constraint in the context of the robot and world.
Arguments:
orientation: The orientation the link should have.
link_id: The name of the link frame.
Returns (OrientationConstraint): The constraint.
"""
constraint = OrientationConstraint()
constraint.header.frame_id = self.frame_id
constraint.link_name = link_id
const_orien = constraint.orientation
const_orien.x, const_orien.y, const_orien.z, const_orien.w = orientation
constraint.absolute_x_axis_tolerance = tolerance
constraint.absolute_y_axis_tolerance = tolerance
constraint.absolute_z_axis_tolerance = tolerance
constraint.weight = weight
return constraint
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped, Pose
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
rospy.init_node("motion_planning")
scene = PlanningSceneInterface()
robot = RobotCommander()
L_arm = MoveGroupCommander("L_arm")
rospy.sleep (1)
#print (robot.get_current_variable_values())
print (L_arm.get_current_pose())
print (L_arm.get_planning_frame())
L_arm.set_planning_time(10);
L_arm.set_pose_reference_frame("omo_L")
L_arm.set_position_target([-0.068, -0.120, 0.520])
print (L_arm.get_end_effector_link())
plan = L_arm.plan()
print (plan)
L_arm.execute(plan)
print (L_arm.get_current_pose(L_arm.get_end_effector_link()))
class PlannerAnnotationParser(AnnotationParserBase):
"""
Parses the annotations files that contains the benchmarking
information.
"""
def __init__(self, path_to_annotation, path_to_data):
super(PlannerAnnotationParser, self).__init__(path_to_annotation,
path_to_data)
self.parse()
self._load_scene()
self._init_planning()
self.benchmark()
def check_results(self, results):
"""
Returns the results from the planner, checking them against any eventual validation data
(no validation data in our case).
"""
return self.planner_data
def _load_scene(self):
"""
Loads the proper scene for the planner.
It can be either a python static scene or bag containing an occupancy map.
"""
scene = self._annotations["scene"]
for element in scene:
if element["type"] == "launch":
self.play_launch(element["name"])
elif element["type"] == "python":
self.load_python(element["name"])
elif element["type"] == "bag":
self.play_bag(element["name"])
for _ in range(150):
rospy.sleep(0.3)
# wait for the scene to be spawned properly
rospy.sleep(0.5)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self.space = test["space"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
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:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
if self.space == "joint":
coordinates = test["goal_joints"]
elif self.space == "pose":
position = test["goal_xyz"]
orientation = test["goal_orientation"]
coordinates = self.create_pose(position, orientation)
elif self.space == "position":
coordinates = test["goal_xyz"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
coordinates, self._annotations["name"] + "-test_" +
str(test_id) + "-" + self.space)
return self.planner_data
def create_pose(self, position, orientation):
pose = Pose()
pose.position.x = position[0]
pose.position.y = position[1]
pose.position.z = position[2]
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
return pose
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def _plan_joints(self, joints, test_name):
# plan to joint target and determine success
self.group.clear_pose_targets()
if self.space == "joint":
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)
elif self.space == "pose":
self.group.set_joint_value_target(joints)
elif self.space == "position":
self.group.set_position_target(joints)
plan = self.group.plan()
plan_time = "N/A"
total_joint_rotation = "N/A"
comp_time = "N/A"
plan_evaluation = "N/A"
plan_success = self._check_plan_success(plan)
if plan_success:
plan_time = self._check_plan_time(plan)
total_joint_rotation = self._check_plan_total_rotation(plan)
plan_evaluation = self.evaluate_plan(plan)
while not self._comp_time:
rospy.sleep(0.5)
comp_time = self._comp_time.pop(0)
self.planner_data.append([
self.planner_id, test_name,
str(plan_success), total_joint_rotation, plan_evaluation,
comp_time, plan_time
])
@staticmethod
def _check_plan_success(plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
@staticmethod
def _check_plan_time(plan):
# find duration of successful plan
number_of_points = len(plan.joint_trajectory.points)
time = plan.joint_trajectory.points[number_of_points -
1].time_from_start.to_sec()
return time
@staticmethod
def _check_plan_total_rotation(plan):
# find total joint rotation in successful trajectory
angles = [0, 0, 0, 0, 0, 0]
number_of_points = len(plan.joint_trajectory.points)
for i in range(number_of_points - 1):
angles_temp = [
abs(x - y)
for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
plan.joint_trajectory.points[i].positions)
]
angles = [x + y for x, y in zip(angles, angles_temp)]
total_angle_change = sum(angles)
return total_angle_change
def evaluate_plan(self, plan):
num_of_joints = len(plan.joint_trajectory.points[0].positions)
weights = numpy.array(sorted(range(1, num_of_joints + 1),
reverse=True))
plan_array = numpy.empty(shape=(len(plan.joint_trajectory.points),
num_of_joints))
for i, point in enumerate(plan.joint_trajectory.points):
plan_array[i] = point.positions
deltas = abs(numpy.diff(plan_array, axis=0))
sum_deltas = numpy.sum(deltas, axis=0)
sum_deltas_weighted = sum_deltas * weights
plan_quality = numpy.sum(sum_deltas_weighted)
return plan_quality
def _check_computation_time(self, msg):
# get computation time for successful plan to be found
if msg.status.status == 3:
self._comp_time.append(msg.result.planning_time)
def _check_path_length(self, plan):
# find distance travelled by end effector
# not yet implemented
return
class MoveitMove(EventState):
'''
Move the arm to a specific pose or point or named_target
-- move wetter or not the arm should move or simply check if the move is possible
-- waitForExecution wait for execution to end before continuing
># pose Pose Targetpose.
<= done No error occurred.
<= failed The plan could't be done.
'''
def __init__(self,
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15):
# See example_state.py for basic explanations.
super(MoveitMove, self).__init__(outcomes=['done', 'failed'],
input_keys=['target'])
self.move = move
self.waitForExecution = waitForExecution
self.group = MoveGroupCommander(group)
self.tol = 0.06
self.result = None
self.count = 0
self.timeout = rospy.Time.now()
self.watchdog = watchdog
def execute(self, userdata):
if self.result:
return self.result
if self.waitForExecution:
curState = self.group.get_current_joint_values()
diff = compareStates(curState, self.endState)
print("diff=" + str(diff))
if diff < self.tol or self.timeout + rospy.Duration(
self.watchdog) < rospy.Time.now():
self.count += 1
if self.count > 3:
Logger.loginfo('Target reached :)')
return "done"
else:
self.count = 0
else:
return "done"
def on_enter(self, userdata):
Logger.loginfo('Enter Move Arm')
self.timeout = rospy.Time.now()
if type(userdata.target) is Pose:
Logger.loginfo('the target is a pose')
self.group.set_pose_target(userdata.target)
elif type(userdata.target) is Point:
Logger.loginfo('the target is a point')
xyz = [userdata.target.x, userdata.target.y, userdata.target.z]
self.group.set_position_target(xyz)
elif type(userdata.target) is str:
Logger.loginfo('the target is a named_target')
self.group.set_named_target(userdata.target)
else:
Logger.loginfo(
'ERROR in ' + str(self.name) +
' : target is not a Pose() nor a Point() nor a string')
self.result = 'failed'
Logger.loginfo('target defined')
try:
plan = self.group.plan()
Logger.loginfo(str(plan))
# Logger.loginfo(str(plan.joint_trajectory.points))
self.endState = plan.joint_trajectory.points[
len(plan.joint_trajectory.points) - 1].positions
Logger.loginfo(str(self.endState))
except:
Logger.loginfo('Planning failed')
self.result = 'failed'
return
Logger.loginfo('Plan done successfully')
if self.move:
Logger.loginfo('Initiating movement')
self.group.execute(plan, wait=False)
if not self.waitForExecution:
self.result = "done"
else:
Logger.loginfo('The target is reachable')
self.result = "done"
def on_exit(self, userdata):
Logger.loginfo('Stoping movement')
self.group.stop()
def on_pause(self):
Logger.loginfo('Pausing movement')
self.group.stop()
def on_resume(self, userdata):
self.on_enter(userdata)
