def execute_trajectory(self, traj):
if self._execution_running:
rospy.logwarn(
'Request to execute a trajectory while an old one is already running'
)
self._action_client.cancel_goal()
self._execution_running = False
traj.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(0.1)
self._current_arm_traj = ROSUtils.extract_part_trajectory(
self._arm_joint_names, traj)
self._current_hand_traj = ROSUtils.extract_part_trajectory(
self._hand_joint_names, traj)
self._current_traj = traj
arm_traj_goal = FollowJointTrajectoryActionGoal()
arm_traj_goal.goal.trajectory = self._current_arm_traj
arm_traj_goal.goal.goal_tolerance = self._goal_tolerance
arm_traj_goal.goal.path_tolerance = self._path_tolerance
# TODO for some reason we do not receive feedback, so we add a fake feedback caller
# TODO here
self._action_client.send_goal(
arm_traj_goal.goal,
active_cb=self._receive_arm_traj_start_signal,
feedback_cb=self._receive_arm_traj_feedback,
done_cb=self._receive_arm_traj_finish_signal)
self._fake_feedback_timer = rospy.Timer(rospy.Duration.from_sec(0.05),
self._simulate_feedback,
oneshot=False)
# TODO return whether we grasped an object
return True
def send_parameters(parameter_description, param_prefix, parameters, client):
params_to_send = {}
for (name, description) in parameter_description.iteritems():
short_name = name.replace(param_prefix + '_', '')
params_to_send[short_name] = parameters[name]
ROSUtils.guarded_service_call(client.update_configuration, params_to_send,
client.name + '/set_parameters')
def __init__(self, parameters):
addNumpyYaml()
self._basePath = ''
self._loadModelPaths(ROSUtils.resolvePath(parameters['modelPathFile']))
self._moveItLaunchFile = ROSUtils.resolvePath(
parameters['moveItLaunchFile'])
moveit_commander.roscpp_initialize(sys.argv)
self._moveit_commander = moveit_commander.RobotCommander()
self._moveit_scene = moveit_commander.PlanningSceneInterface()
self._moveGroups = {}
def set_object_pose(self, object_info):
add_object_request = AddObjectRequest()
add_object_request.object_identifier = object_info.name
add_object_request.class_identifier = object_info.object_class
add_object_request.pose = ROSUtils.toROSPose(object_info.pose,
bStamped=True)
response = ROSUtils.guarded_service_call(self._add_object_service,
add_object_request,
self._add_object_service_name)
if response is None:
return False
return response.success
def set_robot_pose(self, robot_info):
add_object_request = AddObjectRequest()
add_object_request.object_identifier = robot_info.name
add_object_request.class_identifier = robot_info.name
add_object_request.pose = ROSUtils.toROSPose(robot_info.pose,
bStamped=True)
response = ROSUtils.guarded_service_call(self._add_object_service,
add_object_request,
self._add_object_service_name)
if response is None:
return False
# TODO we should also set the configuration here
return response.success
def cleanup_scene(self):
remove_object_request = RemoveObjectRequest()
remove_object_request.object_identifier = 'all'
response = ROSUtils.guarded_service_call(
self._remove_object_service, remove_object_request,
self._remove_object_service_name)
if response is None or not response.success:
raise RuntimeError('Could not clean-up planning scene!')
def _call_hfts_planner(self, object_name, context):
# TODO set parameters
object_info = context.getSceneInformation().getObjectInfo(object_name)
hfts_planner_request = PlanGraspMotionRequest()
hfts_planner_request.object_identifier = object_name
hfts_planner_request.model_identifier = object_info.object_class
hfts_planner_request.object_pose = ROSUtils.toROSPose(object_info.pose,
bStamped=True)
start_config = context.getRobotInformation().configuration
hfts_planner_request.start_configuration = ROSUtils.toJointState(
start_config)
response = ROSUtils.guarded_service_call(
self._integrated_hfts_service, hfts_planner_request,
self._integrated_hfts_service_name)
if response is None or not response.planning_success:
return None, None
return response.trajectory, response.grasp_pose
def planGrasp(self, objectName, context, paramPrefix, parameters):
send_parameters(self.getParameters('GraspPlanner', paramPrefix),
paramPrefix, parameters, self._parameter_client)
object_info = context.getSceneInformation().getObjectInfo(objectName)
plan_grasp_request = PlanGraspRequest()
plan_grasp_request.object_identifier = object_info.object_class
response = ROSUtils.guarded_service_call(self._grasp_planner_service,
plan_grasp_request,
self._service_name)
if response is not None and response.planning_success:
pose = ROSUtils.toContextPose(response.grasp_pose)
configuration = ROSUtils.toContextConfiguration(
response.hand_configuration)
# TODO approach direction?
grasp_result = ContextGraspResult(grasp_pose=pose,
approach_vector=None,
hand_configuration=configuration)
return grasp_result
return None
def _synchEnvironment(self, context):
si = context.getSceneInformation()
# First clear the world
self._moveit_scene.remove_world_object()
# now repopulate
for obj in si.objects:
objModel = self._modelPaths[obj.name]
pose = ROSUtils.contextPoseToROSPose(obj.pose, bStamped=True)
if isinstance(objModel, Box):
self._moveit_scene.add_box(
obj.name, pose,
(objModel.width, objModel.length, objModel.height))
elif isinstance(objModel, Sphere):
self._moveit_scene.add_sphere(obj.name, pose, objModel.radius)
elif isinstance(
objModel, Mesh
): # it has to be a Mesh -> get path to mesh + transform
if self._basePath == '':
meshFile = ROSUtils.resolvePath(objModel.meshFile)
else:
meshFile = self._basePath + objModel.meshFile
# We have to compute the transform from the mesh frame to world frame
eulerAngles = tf.transformations.euler_from_quaternion(
objModel.pose.orientation)
meshToObject = tf.transformations.compose_matrix(
translate=objModel.pose.position, angles=eulerAngles)
eulerAngles = tf.transformations.euler_from_quaternion(
obj.pose.orientation)
objectToWorld = tf.transformations.compose_matrix(
translate=obj.pose.position, angles=eulerAngles)
meshToWorld = numpy.dot(objectToWorld, meshToObject)
orientation = tf.transformations.quaternion_from_matrix(
meshToWorld)
position = tf.transformations.translation_from_matrix(
meshToWorld)
pose = ROSUtils.contextPoseToROSPose(ContextPose(
position=position, orientation=orientation),
bStamped=True)
# print 'Publising object %s at pose %s with mesh %s to MoveIt!' % (obj.name, pose, meshFile)
self._moveit_scene.add_mesh(obj.name, pose, meshFile)
else:
raise RuntimeError('Unknown object model type: %s' % objModel)
def planArmTrajectory(self, goal, context, paramPrefix, parameters):
# If this function is called, it means we are only asked to plan an arm motion
send_parameters(self.getParameters('ArmPlanner', paramPrefix),
paramPrefix, parameters, self._parameter_client)
self._scene_interface.synchronize_scene(context)
plan_arm_request = PlanArmMotionRequest()
start_configuration = context.getRobotInformation().configuration
if type(goal) is not ContextPose:
raise NotImplementedError(
'[IntegratedHFTSPlanner::planArmTrajectory] Goals of type %s not supported.'
% str(type(goal)))
plan_arm_request.target_pose = ROSUtils.toROSPose(goal, bStamped=True)
plan_arm_request.start_configuration = ROSUtils.toJointState(
start_configuration)
plan_arm_request.grasped_object = context.getRobotInformation(
).grasped_object
response = ROSUtils.guarded_service_call(self._plan_arm_motion_service,
plan_arm_request,
self._arm_service_name)
if response is not None and response.planning_success:
return ROSUtils.toContextTrajectory(response.trajectory)
return None
def executeArmTrajectory(self, trajectory, context, paramPrefix,
parameters):
# If this function is called, it means we are only asked to execute an arm motion
send_parameters(self.getParameters('ArmController', paramPrefix),
paramPrefix, parameters, self._parameter_client)
if self._dummy_controller is None:
ros_traj = ROSUtils.toROSTrajectory(trajectory)
ros_traj.header.stamp = rospy.Time.now() + rospy.Duration.from_sec(
0.01)
self._synched_hand_arm_controller.execute_arm_trajectory(
ros_traj, block=True)
return True
else:
return self._dummy_controller.executeArmTrajectory(
trajectory, context, paramPrefix, parameters)
def _loadModelPaths(self, modelPathFile):
f = open(modelPathFile, 'r')
fileContent = f.read()
if fileContent == "":
f.close()
raise IOError('The given model path file %s is empty.' %
modelPathFile)
modelInfo = yaml.load(fileContent)
base_path = ""
# There is either a base path specified in the file
if 'base_path' in modelInfo:
base_path = modelInfo['base_path']
# It might contain $ROS_PACKAGE_PATH macros, so let's resolve these
base_path = ROSUtils.resolvePath(base_path)
# or there is no base path and we assume that we are given absolute paths for each object file
self._basePath = base_path
self._modelPaths = modelInfo['models']
f.close()
def _make_grasp_result(self, trajectory, pose):
if pose is None or trajectory is None:
return None
# Let us extract the grasp configuration from the trajectory
indices = [
i for i in range(len(trajectory.joint_names))
if trajectory.joint_names[i] in self._hand_joint_names
]
grasp_joint_values = [
trajectory.points[-1].positions[i] for i in indices
]
grasp_joint_names = [trajectory.joint_names[i] for i in indices]
# Create output
configuration = dict(zip(grasp_joint_names, grasp_joint_values))
pose = ROSUtils.toContextPose(pose)
# TODO what about approach dir?
approach_dir = None
grasp_result = ContextGraspResult(grasp_pose=pose,
approach_vector=approach_dir,
hand_configuration=configuration)
return grasp_result
def _planMoveItTraj(self, context, goal, paramPrefix, parameters):
self.preparePlanning(context)
moveGroupName = parameters[paramPrefix + '_moveGroup']
algorithmName = parameters[paramPrefix + '_algorithm']
timeOut = parameters[paramPrefix + '_timeout']
longest_valid_segment_fraction = parameters[
paramPrefix + '_longest_valid_segment_fraction']
if moveGroupName not in self._moveGroups:
self._moveGroups[
moveGroupName] = moveit_commander.MoveGroupCommander(
moveGroupName)
moveGroup = self._moveGroups[moveGroupName]
moveGroup.set_planning_time(timeOut)
self._setMoveItROSParameters(paramPrefix, parameters)
moveGroup.clear_pose_targets()
robotState = self._convertRobotState(context,
moveGroup.get_active_joints())
moveGroup.set_start_state(robotState)
if isinstance(goal, ContextPose):
# input goal is pose, transform to ROSPose and send it to Moveit
pose = ROSUtils.contextPoseToROSPose(goal, bStamped=True)
moveGroup.set_pose_target(pose)
elif isinstance(goal, ContextPosition):
# input is just a position, transform to list and send it to Moveit
position = [x for x in goal.position]
moveGroup.set_position_target(position)
elif isinstance(goal, ContextConfiguration):
# if input goal is configuration, plan to config
moveGroup.set_joint_value_target(goal.configuration)
else:
raise ValueError('The given goal %g is of invalid type.' % goal)
# TODO extend so that contraints are respected (e.g. follow cartesian path)
moveGroup.set_planner_id(algorithmName)
moveit_traj = moveGroup.plan()
if len(moveit_traj.joint_trajectory.points) == 0:
# we did not find a solution
return None
return moveit_traj
def executeBatch(self, context, batchInput, parameters, getWorldStateFn):
# We can be sure that the batch consists of [GraspPlanner, ArmPlanner, ArmController, GraspController]
assert len(batchInput) == 3 and batchInput[0][0] == 'GraspPlanner' and batchInput[1][0] == 'ArmPlanner' \
and batchInput[2][0] == 'ArmController'
grasp_planner_input = batchInput[0][1]
grasp_planner_param_prefix = grasp_planner_input['paramPrefix']
arm_planner_input = batchInput[1][1]
arm_planner_param_prefix = arm_planner_input['paramPrefix']
# Send parameters for grasp and arm motion planning (TODO this would be a bit problematic if they intersected)
send_parameters(
self.getParameters('GraspPlanner', grasp_planner_param_prefix),
grasp_planner_param_prefix, parameters, self._parameter_client)
send_parameters(
self.getParameters('ArmPlanner', arm_planner_param_prefix),
arm_planner_param_prefix, parameters, self._parameter_client)
self._scene_interface.synchronize_scene(context)
trajectory, pose = self._call_hfts_planner(
grasp_planner_input['objectName'], context)
if trajectory is None or pose is None:
return 3 * [(False, None)]
results = []
grasp_result = self._make_grasp_result(trajectory, pose)
results.append((True, grasp_result))
context_traj = ROSUtils.toContextTrajectory(trajectory)
results.append((True, context_traj))
# TODO if we had parameters for the controllers, we would need to set these here
if self._dummy_controller is None:
control_success = self._synched_hand_arm_controller.execute_trajectory(
trajectory)
else:
control_success = self._dummy_controller.executeArmTrajectory(
context_traj, context, None, parameters)
# control_success = self._dummy_controller.startGraspExecution(grasp_result, context,
# None, parameters)
results.append((control_success, control_success))
# results.append(control_success, control_success)
return results