def plan(self):
dt = DisplayTrajectory()
dt.trajectory_start = self.robot.get_current_state()
plan = self.group.plan()
dt.trajectory = [plan]
self.dt_pub.publish(dt)
return plan
def display_planned_path(self, path):
"""Sends the trajectory to apropriate topic to be displayed.
Arguments:
path : {RobotTrajectory message} Trajectory to be displayed
"""
self.pub = rospy.Publisher(TOPIC_DISPLAY_PLANNED_PATH,
DisplayTrajectory)
dsp = DisplayTrajectory()
dsp.trajectory = [path]
self.pub.publish(dsp)
# pose_goal.y = 0.1
# pose_goal.z = 0.2
# # random_pose = group.get_random_pose()
# mp_req_2 = create_basic_mp_position_request(
# planning_frame, eef_link, pose_goal, "LIN")
motion_sequence_request = MotionSequenceRequest()
item_1 = MotionSequenceItem()
item_1.blend_radius = 0
item_1.req = mp_req_1
item_2 = MotionSequenceItem()
item_2.blend_radius = 0
item_2.req = mp_req_2
motion_sequence_request = MotionSequenceRequest()
motion_sequence_request.items = [item_1, item_2]
rospy.wait_for_service('plan_sequence_path')
try:
service_call = rospy.ServiceProxy('plan_sequence_path', GetMotionSequence)
result = service_call(motion_sequence_request)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory = result.response.planned_trajectories
# Publish
display_trajectory_publisher.publish(display_trajectory)
except rospy.ServiceException as e:
print ("Service call failed: %s" % e)
def _plan(self, event):
if self._done: return
self._arm.remember_joint_values("HOME")
base_pose = convert.pose2matrix(self._arm.get_current_pose())
home_counter = 0
error_counter = 0
previous_state = copy.deepcopy(self._robot.get_current_state())
while True:
if error_counter > self._max_errors:
self.send_error("PLANNING", [], "Failed too often to plan to next pose")
self._arm.set_start_state(previous_state)
if not self._done or home_counter > 5:
random_translation = transform.random_translation(1.0)
random_translation[0,3] *= self._max_translation[0]
random_translation[1,3] *= self._max_translation[1]
random_translation[2,3] *= self._max_translation[2]
random_rotation = transform.random_rotation(self._max_angle)
new_pose = convert.matrix2pose(random_translation.dot(base_pose).dot(random_rotation))
self._arm.set_pose_target(new_pose)
home_counter = 0
else:
self._arm.set_named_target("HOME")
home_counter += 1
print("Planning home: {}".format(home_counter))
# do the planning
plan = self._arm.plan()
# replan if trajectory constraints not satified
points = len(plan.joint_trajectory.points)
if points <= 0:
error_counter += 1
continue
if points > 15:
error_counter += 1
print("Complex trajectory with {} points".format(points))
continue
error_counter = 0
# scale trajectory speed
self._scale_trajectory(plan, self._speed)
# display plan
trajectoryMsg = DisplayTrajectoryMsg()
trajectoryMsg.trajectory_start = previous_state
trajectoryMsg.trajectory = [plan]
self._pubMove.publish(trajectoryMsg)
# update robot state
previous_state = copy.deepcopy(self._robot.get_current_state())
previous_state.joint_state.position = list(previous_state.joint_state.position)
for i, joint in enumerate(plan.joint_trajectory.joint_names):
idx = previous_state.joint_state.name.index(joint)
previous_state.joint_state.position[idx] = plan.joint_trajectory.points[-1].positions[i]
# add plan
self._queue.put(plan)
# stop after plan home found
if self._done and home_counter > 0:
break
self._arm.forget_joint_values("HOME")
