def ExecutePlan(plan):
ac_ = actionlib.SimpleActionClient("/execute_trajectory",
ExecuteTrajectoryAction)
goal = ExecuteTrajectoryGoal()
goal.trajectory = plan
result = ac_.send_goal_and_wait(goal, execute_timeout=rospy.Duration(60.0))
return result.error_code.val
def move_pickup_raise(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.8 * self.speed_scalar, [], [])
result = None
self.state = "pickup_raise"
goal = ExecuteTrajectoryGoal()
goal.trajectory = self.plan_dictionary['pickup_raise']
self.execute_trajectory_action.send_goal(goal,
active_cb=self._active_client,
done_cb=self._finished_client)
def ExecutePlan(plan):
ac_ = actionlib.SimpleActionClient("/execute_trajectory",
ExecuteTrajectoryAction)
ac_.wait_for_server()
goal = ExecuteTrajectoryGoal()
goal.trajectory = plan
goal.trajectory.joint_trajectory.header.stamp = rospy.Time.now()
result = ac_.send_goal_and_wait(goal, execute_timeout=rospy.Duration(60.0))
return result
def move_pickup_lower(self):
self.controller_commander.set_controller_mode(
self.desired_controller_mode, 0.8 * self.speed_scalar, [],
self.get_payload_pickup_ft_threshold(self.current_target))
result = None
self.state = "pickup_lower"
goal = ExecuteTrajectoryGoal()
goal.trajectory = self.plan_dictionary['pickup_lower']
self.execute_trajectory_action.send_goal(goal,
active_cb=self._active_client,
done_cb=self._finished_client)
def move_pickup_grab_first_step(self):
self.controller_commander.set_controller_mode(self.desired_controller_mode, 0.4*self.speed_scalar, [],\
self.get_payload_pickup_ft_threshold(self.current_target))
result = None
self.state = "pickup_grab_first_step"
try:
goal = ExecuteTrajectoryGoal()
goal.trajectory = self.plan_dictionary['pickup_grab_first_step']
self.execute_trajectory_action.send_goal(
goal,
active_cb=self._active_client,
done_cb=self._finished_client)
#self.controller_commander.execute(self.plan_dictionary['pickup_grab_first_step'])
except Exception as err:
print err
def on_enter(self, userdata):
self._planning_failed = False
self._control_failed = False
self._success = False
# perform planing using moveit_commander
try:
# start state
self._group.set_start_state_to_current_state()
# clear old targets and constraints
self._group.clear_pose_targets()
self._group.clear_path_constraints()
# set target
# TODO set_joint_value_target is not working
# TODO check userdata.pose type
pose = PoseStamped(header=userdata.pose.header,
pose=userdata.pose.pose)
# self._group.set_joint_value_target(pose, True)
self._group.set_pose_target(pose)
# plan
robot_trajectory = self._group.plan()
except MoveItCommanderException as e:
Logger.logwarn('Unable to plan trajectory for %s:\n%s' %
(self._group.get_name(), str(e)))
self._planning_failed = True
return
# another way to check failed planning?
if not robot_trajectory or (len(
robot_trajectory.joint_trajectory.points) == 0 and len(
robot_trajectory.multi_dof_joint_trajectory.points) == 0):
Logger.logwarn(
'Unable to plan trajectory: move_commander.plan() has returned empty trajectory.'
)
self._planning_failed = True
return
# request trajectory execution
action_goal = ExecuteTrajectoryGoal()
action_goal.trajectory = robot_trajectory
try:
self._client.send_goal('execute_trajectory', action_goal)
except Exception as e:
Logger.logwarn(
'Failed to send ExecuteTrajectory goal for group: %s\n%s' %
(self._group.get_name(), str(e)))
self._control_failed = True
def _set_joint_positions(self, goal):
try:
moveit_plan = self._compute_plan(goal.values)
if len(moveit_plan.joint_trajectory.points) < 1:
rospy.logwarn("No motion plan found. No execution attempted")
self.moveit_server_.set_aborted()
return
action_req = ExecuteTrajectoryGoal()
action_req.trajectory = moveit_plan
self._traj_action.send_goal(action_req)
except:
rospy.logerr("PyRobot-Moveit:Unexpected error in move_ee_xyx")
self.moveit_server_.set_aborted()
return
self.wait()
def _move_ee_xyz(self, goal):
try:
(moveit_plan, fraction) = self.moveit_group.compute_cartesian_path(
goal.waypoints, goal.eef_step, 0.0 # waypoints to follow # eef_step
) # jump_threshold
if len(moveit_plan.joint_trajectory.points) < 1:
rospy.logwarn("No motion plan found. No execution attempted")
self.moveit_server_.set_aborted()
return
action_req = ExecuteTrajectoryGoal()
action_req.trajectory = moveit_plan
self._traj_action.send_goal(action_req)
except:
rospy.logerr("PyRobot-Moveit:Unexpected error in move_ee_xyx")
self.moveit_server_.set_aborted()
return
self.wait()
def followCartesian(self,
way_points,
way_point_frame,
max_step,
jump_threshold=0,
link_name=None, #usually it is Gripper Frame
start_state=None, #of type moveit robotstate
avoid_collisions=True):
'''
Movegroup-based cartesian path Control.
:param way_points: waypoints that the robot needs to track
:param way_point_frame: the frame in which the waypoints are given.
:param max_step: resolution (m) of the interpolation
on the cartesian path
:param jump_treshold: a distance in joint space that, if exceeded between
consecutive points, is interpreted as a jump in IK solutions.
:param link_name: frame or link name for which cartesian trajectory
should be followed
:param start_state: robot start state of cartesian trajectory
:param avoid_collisions: if enabled, produces collision free cartesian
path
:type way_points: list of ros message objests of type "Pose"
:type way_point_frame: string
:type max_step: float
:type jump_threshold: float
:type link_name: string
:type start_state: moveit_msgs/RobotState
:type avoid_collisions: bool
'''
req = GetCartesianPathRequest()
req.header.stamp = rospy.Time.now()
req.header.frame_id = way_point_frame
req.group_name = self._group
req.waypoints = way_points
req.max_step = max_step
req.jump_threshold = jump_threshold
req.avoid_collisions = avoid_collisions
if start_state is None:
req.start_state.is_diff = True
else:
req.start_state = start_state
if link_name is not None:
req.link_name = link_name
result = self._cart_service(req)
rospy.loginfo("Cartesian plan for %f fraction of path", result.fraction)
if len(result.solution.joint_trajectory.points) < 1:
rospy.logwarn('No motion plan found. No execution attempted')
return False
rospy.loginfo('Executing Cartesian Plan...')
# 13. send Trajectory
action_req = ExecuteTrajectoryGoal()
action_req.trajectory = result.solution
self._traj_action.send_goal(action_req)
try:
self._traj_action.wait_for_result()
result = self._traj_action.get_result()
return processResult(result)
except:
rospy.logerr('Failed while waiting for action result.')
return False