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 visualize_movement(start, path):
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory,
queue_size=100) # for visualizing the robot movement;
sleep(0.5)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = convertStateToRobotState(start)
trajectory = RobotTrajectory()
points = []
joint_trajectory = JointTrajectory()
joint_trajectory.joint_names = get_joint_names('right')
for state, _ in path:
point = JointTrajectoryPoint()
point.positions = convertStateToRobotState(state).joint_state.position
points.append(point)
joint_trajectory.points = points
trajectory.joint_trajectory = joint_trajectory
# print("The joint trajectory is: %s" % trajectory)
display_trajectory.trajectory.append(trajectory)
display_trajectory_publisher.publish(display_trajectory)
def displayTrajFromPlan(self, plan, joint_names, initial_state):
"""Given a plan (GetDMPPlanResponse), joint_names list and an initial_state as RobotState
Create a displayTraj message"""
dt = DisplayTrajectory()
dt.trajectory.append( self.robotTrajectoryFromPlan(plan, joint_names) )
dt.trajectory_start = self.robotStateFromJoints(joint_names, initial_state)
dt.model_id = "reem"
return dt
def visualize_plan(self, plan): # Untested
"""
Visualize the plan in RViz
"""
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = plan.points[0]
display_trajectory.trajectory.extend(plan.points)
self.display_planned_path_publisher.publish(display_trajectory)
def get_display_trajectory(self, *joint_trajectories):
display_trajectory = DisplayTrajectory()
display_trajectory.model_id = 'pr2'
for joint_trajectory in joint_trajectories:
robot_trajectory = RobotTrajectory()
robot_trajectory.joint_trajectory = joint_trajectory
# robot_trajectory.multi_dof_joint_trajectory = ...
display_trajectory.trajectory.append(robot_trajectory)
display_trajectory.trajectory_start = self.get_robot_state()
return display_trajectory
def publish_trajectory(self, plan):
# type: (RobotTrajectory) -> None
"""
Publish trajectory so that we can visualize in Rviz
:param plan:
:return: None
"""
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.get_robot_state()
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
def visualize_path(self, path):
'''
path will be a list of grid cells
'''
current_state = self.robot.get_current_state()
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = current_state
path_config = [self._grid_to_continuous(cell) for cell in path]
display_trajectory.trajectory.append(self.get_robot_trajectory_msg(
path_config))
self.display_trajectory_publisher.publish(display_trajectory)
def _visualize_plan(self, data_store):
# Pull out the plan from the data
full_plan = [x.plan for x in data_store[self.PLAN_OBJ_KEY]]
plan = self._merge_plans(full_plan)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.arm_planner.robot.get_current_state(
)
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
return plan
def moveArmCartesian(self, config):
step = 0.01
attempts = 1
waypoints = []
pose_goal = self.group.get_current_pose().pose
waypoints.append(pose_goal) # current pose
pose_goal.orientation = geometry_msgs.msg.Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
0., -math.pi / 2, 0.))
pose_goal.position.x = config.x
pose_goal.position.y = config.y
pose_goal.position.z = config.z
waypoints.append(copy.deepcopy(pose_goal)) # goal pose
print "- Planning Move..."
# create cartesian plan based on current and goal pose
# compute_cartesian_path: 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 resulting path.
# The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory.
# avoid_collision = True
(plan,
fraction) = self.group.compute_cartesian_path(waypoints, step, 0.0)
rospy.sleep(0.5)
print "%% success:", fraction
while (fraction < 0.8) and (
attempts <= 4
): # force a somewhat successful plan by adding more intermediary points
attempts += 1
step = step / 2
print "%% success:", fraction
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, step, 0.0)
rospy.sleep(0.5)
# check if the move was successful
if fraction > 0.8:
print "Success.-"
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state(
)
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(1)
print "- Executing..."
self.group.execute(plan, wait=True)
rospy.sleep(1)
self.reachable = True
else:
print "Fail.-"
self.reachable = False
def execute(self, traj, wait=True, display=True):
""" Execute the trajectory on the robot arm """
if traj:
rospy.loginfo("Executing trajectory")
if display:
# Display the planned trajectory
display_traj = DisplayTrajectory()
display_traj.trajectory_start = self.robot.get_current_state()
display_traj.trajectory.append(traj)
self.display_trajectory_pub.publish(display_traj)
# Execute the trajectory
self.arm_group.execute(traj, wait=wait)
def plan(samplerIndex, start, goal, ss, space, display_trajectory_publisher):
si = ss.getSpaceInformation()
if samplerIndex == 1:
# use obstacle-based sampling
space.setStateSamplerAllocator(
ob.StateSamplerAllocator(allocSelfCollisionFreeStateSampler))
ss.setStartAndGoalStates(start, goal)
# create a planner for the defined space
planner = og.FMT(si) # change this to FMT;
if samplerIndex == 1:
planner.setVAEFMT(
1) # This flag is for turning on sampling with the VAE generator;
# set parameter;
planner.setExtendedFMT(
False) # do not extend if the planner does not terminate;
planner.setNumSamples(100)
planner.setNearestK(False)
planner.setCacheCC(True)
planner.setHeuristics(True)
# planner.setNearestK(1) # Disable K nearest neighbor implementation;
ss.setPlanner(planner)
start_time = time.time()
solved = ss.solve(40.0)
elapsed_time = time.time() - start_time
if solved:
print("Found solution after %s seconds:" % elapsed_time)
# print the path to screen
path = ss.getSolutionPath()
# ("The solution is: %s" % path)
# Visualization
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = convertStateToRobotState(start)
trajectory = convertPlanToTrajectory(path)
display_trajectory.trajectory.append(trajectory)
display_trajectory_publisher.publish(display_trajectory)
print("Visualizing trajectory...")
sleep(0.5)
else:
print("No solution found after %s seconds: " % elapsed_time)
return elapsed_time, float((int(solved)))
def handle_show_request(self, req):
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
#build moveit msg for display
joint_multi_traj_msg = MultiDOFJointTrajectory()
robot_traj_msg = RobotTrajectory()
robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY
robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg
display_trajectory.trajectory.append(robot_traj_msg)
rospy.loginfo("PlanningService::handle_show_request() -- showing trajectory %s", req.JOINT_TRAJECTORY)
self.display_trajectory_publisher.publish(display_trajectory);
return True
def plan_trajectory_in_cspace(self, goal, visualization=True):
## Then, we will get the current set of joint values for the group
self.group.set_joint_value_target(goal)
plan = self.group.plan()
if visualization:
print "============ Visualizing trajectory_plan"
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state(
)
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
print "============ Waiting while plan is visualized (again)..."
rospy.sleep(5)
return plan
def execute(self):
rospy.loginfo("Start Task")
# Get latest task plan
plan = self.task_q[0]
for points in plan.trajectory.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
scaled_vel = []
scaled_acc = []
for vel in points.velocities:
scaled_vel.append(vel * self.exe_speed_rate)
points.velocities = tuple(scaled_vel)
for acc in points.accelerations:
scaled_acc.append(acc * self.exe_speed_rate *
self.exe_speed_rate)
points.accelerations = tuple(scaled_acc)
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.trajectory.joint_trajectory.joint_names[:]
start_state.position = plan.trajectory.joint_trajectory.points[
-1].positions[:]
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "vs087"
pub_display_msg.trajectory.append(plan.trajectory)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = ExecutePlanAction().action_goal.goal
goal.planning_time = plan.planning_time
goal.start_state = plan.start_state
# goal.start_state.joint_state.header.stamp = rospy.Time.now()
goal.trajectory = plan.trajectory
# rospy.logwarn(goal)
self.client.send_goal(goal)
self.client.wait_for_result()
# Update the task queue
self.task_q.pop(0)
rospy.loginfo("End Task")
def visualize_trajectory(self, blocking=True):
if not self.display:
return
if not self.single_arm:
self.adjust_traj_length()
if self.rviz_pub.get_num_connections() < 1:
rospy.logerr("Rviz topic not subscribed")
return
msg = DisplayTrajectory()
msg.trajectory_start = self.robot_model.get_current_state()
traj = RobotTrajectory()
goal = JointTrajectory()
goal.joint_names = self.left.JOINT_NAMES[:]
if not self.single_arm:
goal.joint_names += self.right.JOINT_NAMES[:]
# The sim is very slow if the number of points is too large
steps = 1 if len(self.left._goal.trajectory.points) < 100 else 10
for idx in range(0, len(self.left._goal.trajectory.points), steps):
comb_point = JointTrajectoryPoint()
lpt = self.left._goal.trajectory.points[idx]
comb_point.positions = lpt.positions[:]
if not self.single_arm:
comb_point.positions += self.right._goal.trajectory.points[idx].positions[:]
comb_point.time_from_start = lpt.time_from_start
goal.points.append(comb_point)
traj.joint_trajectory = goal
msg.trajectory.append(traj)
duration = goal.points[-1].time_from_start.to_sec()
self.rviz_pub.publish(msg)
if blocking:
rospy.loginfo("Waiting for trajectory animation {} seconds".format(duration))
rospy.sleep(duration)
def visualize_path(robot_trajectory, position, planned_path_pub, goal_pos_pub):
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_trajectory)
disp_traj.trajectory_start = RobotState()
planned_path_pub.publish(disp_traj)
marker = Marker()
marker.header.frame_id = "base"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
print 'Goal position {0}'.format(position)
goal_pos_pub.publish(marker)
def execute(self):
# Get latest task plan
plan = self.task_q[0].trajectory
for points in plan.joint_trajectory.points:
tfs = points.time_from_start.to_sec()
tfs /= self.exe_speed_rate
points.time_from_start = rospy.Duration(tfs)
self.grasp_[0] = self.task_q[0].grasp
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.joint_trajectory.joint_names[:]
start_state.position = plan.joint_trajectory.points[-1].positions[:]
moveit_start_state = RobotState()
moveit_start_state.joint_state = start_state
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "sia5"
pub_display_msg.trajectory.append(plan)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = FollowJointTrajectoryGoal(trajectory=plan.joint_trajectory)
rospy.loginfo("Start Task")
self.client.send_goal(goal)
self.client.wait_for_result()
# Grasping
if self.grasp_[0] != self.grasp_[1]:
self.executeGrasp(self.grasp_[0])
self.grasp_[1] = self.grasp_[0]
# Update the task queue
self.task_q.pop(0)
rospy.loginfo("End Task")
def display_trajectory(self, plan):
print(plan)
traj = plan.trajectory
joints = traj.joint_names
print(traj)
init_joints = JointState()
init_joints.name = joints
init_joints.position = traj.points[0].positions
init_joints.velocity = traj.points[0].velocities
init_joints.effort = traj.points[0].effort
joint_traj = RobotTrajectory()
joint_traj.joint_trajectory = plan.trajectory
init_state = RobotState()
init_state.joint_state = init_joints
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = init_state
display_trajectory.trajectory.append(joint_traj)
# Publish
self.display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(3)
def moveArm(self, config):
print "- Planning Move..."
pose_goal = self.group.get_current_pose().pose
pose_goal.orientation = geometry_msgs.msg.Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
0., -math.pi / 2, 0.))
pose_goal.position.x = config.x
pose_goal.position.y = config.y
pose_goal.position.z = config.z
self.group.set_pose_target(pose_goal)
plan = self.group.plan()
rospy.sleep(0.5)
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(1)
print "- Executing..."
self.group.go(wait=True)
rospy.sleep(1)
def main():
"""
python scripts/main.py --help <------This prints out all the help messages
and describes what each parameter is.
NOTE: If running with the --moveit flag, it makes no sense
to also choose your controller to be workspace, since moveit
cannot accept workspace trajectories. This script simply ignores
the controller selection if you specify both --moveit and
--controller_name workspace, so if you want to run with moveit
simply leave --controller_name as default.
You can also change the rate, timeout if you want
"""
parser = argparse.ArgumentParser()
parser.add_argument(
'-task',
'-t',
type=str,
default='line',
help='Options: line, circle, arbitrary. Default: line')
parser.add_argument(
'-goal_point',
'-g',
type=float,
default=[0, 0, 0],
nargs=3,
help=
'Specify a goal point for line and circle trajectories. Ex: -g 0 0 0')
parser.add_argument(
'-controller_name',
'-c',
type=str,
default='jointspace',
help='Options: workspace, jointspace, or torque. Default: jointspace')
parser.add_argument('-arm',
'-a',
type=str,
default='left',
help='Options: left, right. Default: left')
parser.add_argument('-rate',
type=int,
default=200,
help="""
(Hz) This specifies how many loop iterations should occur per second.
It is important to use a rate
and not a regular while loop because you want the loop to refresh at a
constant rate, otherwise you would have to tune your PD parameters if
the loop runs slower / faster. Default: 200""")
parser.add_argument(
'-timeout',
type=int,
default=None,
help=
"""after how many seconds should the controller terminate if it hasn\'t already.
Default: None""")
parser.add_argument(
'-num_way',
type=int,
default=300,
help=
'How many waypoints for the :obj:`moveit_msgs.msg.RobotTrajectory`. Default: 300'
)
parser.add_argument(
'--moveit',
action='store_true',
help=
"""If you set this flag, moveit will take the path you plan and execute it on
the real robot""")
parser.add_argument('--log',
action='store_true',
help='plots controller performance')
args = parser.parse_args()
if args.moveit:
# We can't give moveit a workspace trajectory, so we make it
# impossible.
args.controller_name = 'jointspace'
rospy.init_node('moveit_node')
# this is used for sending commands (velocity, torque, etc) to the robot
limb = baxter_interface.Limb(args.arm)
# Choose an inverse kinematics solver. If you leave ik_solver as None, then the default
# KDL solver will be used. Otherwise, you can uncomment the next line which sets ik_solver.
# Then, the TRAC-IK inverse kinematics solver will be used. If you have trouble with inconsistency
# or poor solutions with one method, feel free to try the other one.
ik_solver = None
# ik_solver = IK("base", args.arm + "_gripper")
# A KDL instance for Baxter. This can be used for forward and inverse kinematics,
# computing jacobians etc.
kin = baxter_kinematics(args.arm)
# This is a wrapper around MoveIt! for you to use.
planner = PathPlanner('{}_arm'.format(args.arm))
# Get an appropriate RobotTrajectory for the task (circular, linear, or arbitrary MoveIt)
# If the controller is a workspace controller, this should return a trajectory where the
# positions and velocities are workspace configurations and velocities. If the controller
# is a jointspace or torque controller, it should return a trajectory where the positions
# and velocities are the positions and velocities of each joint.
robot_trajectory = get_trajectory(limb, kin, ik_solver, planner, args)
if args.controller_name == "workspace":
config = robot_trajectory.joint_trajectory.points[0].positions
pose = create_pose_stamped_from_pos_quat(
[config[0], config[1], config[2]], # XYZ location
[config[3], config[4], config[5], config[6]
], # XYZW quaterion orientation
'base')
success, plan, time_taken, error_code = planner.plan_to_pose(pose)
planner.execute_plan(plan)
else:
start = robot_trajectory.joint_trajectory.points[0].positions
while not rospy.is_shutdown():
try:
# ONLY FOR EMERGENCIES!!! DON'T UNCOMMENT THE NEXT LINE UNLESS YOU KNOW WHAT YOU'RE DOING!!!
# limb.move_to_joint_positions(joint_array_to_dict(start, limb), timeout=7.0, threshold=0.0001)
success, plan, time_taken, error_code = planner.plan_to_joint_pos(
start)
planner.execute_plan(plan)
break
except moveit_commander.exception.MoveItCommanderException as e:
print(e)
print("Failed planning, retrying...")
if args.moveit:
# PROJECT 1 PART A
# by publishing the trajectory to the move_group/display_planned_path topic, you should
# be able to view it in RViz. You will have to click the "loop animation" setting in
# the planned path section of MoveIt! in the menu on the left side of the screen.
pub = rospy.Publisher('move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_trajectory)
disp_traj.trajectory_start = RobotState()
pub.publish(disp_traj)
try:
input('Press <Enter> to execute the trajectory using MOVEIT')
except KeyboardInterrupt:
sys.exit()
# uses MoveIt! to execute the trajectory. make sure to view it in RViz before running this.
# the lines above will display the trajectory in RViz
if args.log:
node = roslaunch.core.Node("proj1_pkg",
"moveit_plot.py",
args="{} {}".format(
args.arm, args.rate))
launch = roslaunch.scriptapi.ROSLaunch()
launch.start()
process = launch.launch(node)
rospy.wait_for_service('start_logging')
start_service = rospy.ServiceProxy('start_logging', TriggerLogging)
request = TriggerLoggingRequest(path=robot_trajectory)
start_service(robot_trajectory)
planner.execute_plan(robot_trajectory)
if args.log:
r = rospy.Rate(1)
while process.is_alive():
r.sleep()
else:
# PROJECT 1 PART B
controller = get_controller(args.controller_name, limb, kin)
try:
input(
'Press <Enter> to execute the trajectory using YOUR OWN controller'
)
except KeyboardInterrupt:
sys.exit()
# execute the path using your own controller.
done = controller.execute_path(robot_trajectory,
rate=args.rate,
timeout=args.timeout,
log=args.log)
if not done:
print('Failed to move to position')
sys.exit(0)
def visualize_plan(self, plan):
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = plan.points[0]
display_trajectory.trajectory.extend(plan.points)
self.display_planned_path_publisher.publish(display_trajectory)
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")
def __init__(self):
#关于baxter无法通过moveit获取当前姿态的错误 https://github.com/ros-planning/moveit/issues/1187
#joint_state_topic = ['joint_states:=/robot/joint_states']
#初始化moveit的 API接口
moveit_commander.roscpp_initialize(sys.argv)
#初始化ros节点，
rospy.init_node('panda_grasp', anonymous=True)
#构建一个tf发布器
self.tf_broadcaster = tf.TransformBroadcaster()
self.grasp_config = GraspConfig()
#创建一个TF监听器
self.tf_listener = tf.TransformListener()
#一直等待接收到桌面标签和机器人base坐标系之间的变换（需要提前进行手眼标定）
get_transform = False
while not get_transform:
try:
#尝试查看机器人基座base与桌面标签之间的转换
trans, rot = self.tf_listener.lookupTransform(
'/panda_link0', '/ar_marker_6', rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(rot)
self.base2marker = tf.transformations.compose_matrix(
translate=trans, angles=euler)
#查看gripper到link8之间的变换
trans, rot = self.tf_listener.lookupTransform(
'/panda_EE', '/panda_link8', rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(rot)
self.gripper2link8 = tf.transformations.compose_matrix(
translate=trans, angles=euler)
#查看base到panda_link8的变换，此时就是查询gripper的初始姿态
trans, rot = self.tf_listener.lookupTransform(
'/panda_link0', '/panda_link8', rospy.Time(0))
euler = tf.transformations.euler_from_quaternion(rot)
self.base2Initial_link8 = tf.transformations.compose_matrix(
translate=trans, angles=euler)
get_transform = True
rospy.loginfo("got transform complete")
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("got transform failed")
rospy.sleep(0.5)
continue
# 初始化场景对象
scene = moveit_commander.PlanningSceneInterface()
rospy.sleep(2)
# 创建机械臂规划组对象
self.selected_arm = moveit_commander.MoveGroupCommander('panda_arm')
#创建机械手规划对象
hand_group = moveit_commander.MoveGroupCommander('hand')
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
DisplayTrajectory,
queue_size=20)
# 获取左臂末端执行器名称
self.end_effector_link = self.selected_arm.get_end_effector_link()
print("i am here")
print(self.end_effector_link)
# 创建机械臂父坐标系名称字符
reference_frame = 'panda_link0'
#self.tf_listener = tf.TransformListener()
# 设置父坐标系名称
#self.selected_arm.set_pose_reference_frame(reference_frame)
# 允许机械臂进行重规划
#self.selected_arm.allow_replanning(True)
# 允许机械臂末位姿的错误余量
self.selected_arm.set_goal_position_tolerance(0.01)
self.selected_arm.set_goal_orientation_tolerance(0.05)
#不允许规划失败重规划,规划时间只允许5秒钟,否则很浪费时间
self.selected_arm.allow_replanning(False)
self.selected_arm.set_planning_time(5)
#清除之前遗留的物体
scene.remove_world_object('table')
#设置桌面高度
table_ground = 0.6
#桌面尺寸 x y z
table_size = [0.6, 1.2, 0.01]
# 将table加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = 'panda_link0'
table_pose.pose.position.x = 0.55
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = 0.025
table_pose.pose.orientation.w = 1.0
scene.add_box('table', table_pose, table_size)
#设置初始姿态
#Home_positions = [-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50]#移动到工作位置，使用正运动学
# Set the arm's goal configuration to the be the joint positions
#self.selected_arm.set_joint_value_target(Home_positions)
# Plan and execute the motion
#self.selected_arm.go()
#rospy.sleep(5)
# 设置home姿态
Home_positions = [0.04, -0.70, 0.18, -2.80, 0.19, 2.13,
0.92] #移动到工作位置，使用正运动学
#self.selected_arm.remember_joint_values('resting', joint_positions)#存储当前状态为初始状态
#self.start_state =self.selected_arm.get_current_pose()
# Set the arm's goal configuration to the be the joint positions
self.selected_arm.set_joint_value_target(Home_positions)
# Plan and execute the motion，运动到Home位置
self.selected_arm.go()
self.selected_arm.stop()
joint_goal = hand_group.get_current_joint_values()
joint_goal[0] = 0.04
joint_goal[1] = 0.04
hand_group.go(joint_goal, wait=True)
hand_group.stop()
######################开始等待接收夹爪姿态#########################
print("Waiting for gripper pose!")
#等待gripper_pose这个话题的发布（目标抓取姿态，该姿态将会进行抓取）
#rospy.wait_for_message('/detect_grasps/clustered_grasps', GraspConfigList)
#创建消息订阅器，订阅“gripper_pose”话题，这个gripper_pose，是以桌面标签为参考系的
#接收到之后，调用回调函数，有两件事要做
# 1.将gripper_pose
# 计算后撤距离
self.r_flag = False
rospy.Subscriber('/detect_grasps/clustered_grasps',
GraspConfigList,
self.Callback,
queue_size=1)
#能不能，先检查并读取 桌面标签和机器人底座之间的转换关系？（这只是其中一条路吧）
#利用TF直接，读取grasp_pose在base坐标系下面的姿态
#调用pose处理函数，计算预抓取姿态，
#rospy.sleep(5)
print("Start to Recieve Grasp Config!")
#####################################################################
while not rospy.is_shutdown():
if self.r_flag:
#目标姿态设定为预抓取位置
#self.pre_grasp_pose
self.r_flag = False
#尝试利用TF直接，读取grasp_pose在base坐标系下面的姿态
#计算预抓取姿态
else:
rospy.sleep(0.5)
continue
#以当前姿态作为规划起始点
self.selected_arm.set_start_state_to_current_state()
# 对末端执行器姿态设定目标姿态
#self.selected_arm.set_pose_target(target_pose, 'left_gripper')
# 规划轨迹
#traj = self.selected_arm.plan(target_pose.pose)
# 执行轨迹，运行到预抓取位置
#self.selected_arm.execute(traj)
print(self.end_effector_link)
print('Moving to pre_grasp_pose')
#self.selected_arm.pick("test",self.grasp_config,plan_only = True)
#traj=self.selected_arm.plan(self.pre_grasp_pose)
#self.selected_arm.set_pose_target(self.pre_grasp_pose,end_effector_link="panda_EE")
#traj=self.selected_arm.plan()
#continue
#success=self.selected_arm.execute(traj)
#print(target_pose.pose)
#设置规划
#self.selected_arm.set_planning_time(5)
success = self.selected_arm.go(self.pre_grasp_pose, wait=True)
self.selected_arm.stop()
self.selected_arm.clear_pose_targets()
if not success:
print('Failed to move to pre_grasp_pose!')
continue
print('Move to pre_grasp_pose succeed')
#等待机械臂稳定
rospy.sleep(1)
#再设置当前姿态为起始姿态
self.selected_arm.set_start_state_to_current_state()
#
waypoints = []
wpose = self.selected_arm.get_current_pose().pose
#print("#####wpose.position")
#print(wpose.position)
#print("#####self.grasp_pose2")
#print(self.grasp_pose.position)
wpose.position.x = self.grasp_pose.position.x
wpose.position.y = self.grasp_pose.position.y
wpose.position.z = self.grasp_pose.position.z
waypoints.append(copy.deepcopy(wpose))
#wpose = self.selected_arm.get_current_pose().pose
#wpose.position.z -= scale * 0.1
#规划从当前位姿，保持姿态，转移到目标夹爪姿态的路径
(plan, fraction) = self.selected_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
##显示轨迹
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.selected_arm.get_current_state(
)
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
#执行,并等待这个轨迹执行成功
new_plan = self.scale_trajectory_speed(plan, 0.3)
self.selected_arm.execute(new_plan, wait=True)
#self.selected_arm.shift_pose_target(2,0.05,"panda_link8")
#self.selected_arm.go()
#执行抓取
rospy.sleep(2)
print("Grasping")
joint_goal = hand_group.get_current_joint_values()
joint_goal[0] = 0.015
joint_goal[1] = 0.015
#plan=hand_group.plan(joint_goal)
#new_plan=self.scale_trajectory_speed(plan,0.3)
hand_group.go(joint_goal, wait=True)
hand_group.stop()
####################抓取完后撤####################
waypoints = []
wpose = self.selected_arm.get_current_pose().pose
wpose.position.x = self.pre_grasp_pose.position.x
wpose.position.y = self.pre_grasp_pose.position.y
wpose.position.z = self.pre_grasp_pose.position.z
waypoints.append(copy.deepcopy(wpose))
#规划从当前位姿，保持姿态，转移到目标夹爪姿态的路径
(plan, fraction) = self.selected_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
#执行,并等待后撤成功
new_plan = self.scale_trajectory_speed(plan, 0.6)
self.selected_arm.execute(new_plan, wait=True)
"""
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.selected_arm.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
"""
######################暂时设置直接回到Home############################
#self.selected_arm.remember_joint_values('resting', joint_positions)#存储当前状态为初始状态
#self.start_state =self.selected_arm.get_current_pose(self.end_effector_link)
# Set the arm's goal configuration to the be the joint positions
self.selected_arm.set_joint_value_target(Home_positions)
# Plan and execute the motion，运动到Home位置
self.selected_arm.go()
self.selected_arm.stop()
joint_goal = hand_group.get_current_joint_values()
joint_goal[0] = 0.04
joint_goal[1] = 0.04
hand_group.go(joint_goal, wait=True)
hand_group.stop()
print("Grasp done")
rospy.sleep(5)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def poseCallback(Pose_Array):
print type(Pose_Array)
posearray=Pose_Array.posearray
print (len(posearray))
i=80
for pose in posearray[80:]:
i=i+1
data=pose.pose
#print (type(data))
print (data)
try:
dataCopy1 = copy.deepcopy(data)
(r, p, y) = tf.transformations.euler_from_quaternion([data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w])
tMatrix = tf.transformations.euler_matrix(r, p, y)
x_offset=-0.10
data.pose.position.x=data.pose.position.x+(tMatrix[0,0]*x_offset)
data.pose.position.y=data.pose.position.y + (tMatrix[1,0]*x_offset)
data.pose.position.z=data.pose.position.z + (tMatrix[2,0]*x_offset)
newRotMatrix = tf.transformations.euler_matrix(0, 1.57, 0)
transformedMatrix = numpy.dot(tMatrix, newRotMatrix)
dataCopy1.pose.position.x=dataCopy1.pose.position.x - (transformedMatrix[0,2]*x_offset)
dataCopy1.pose.position.y=dataCopy1.pose.position.y - (transformedMatrix[1,2]*x_offset)
dataCopy1.pose.position.z=dataCopy1.pose.position.z - (transformedMatrix[2,2]*x_offset)
dis = (dataCopy1.pose.position.x - data.pose.position.x) * (dataCopy1.pose.position.x - data.pose.position.x)
dis = dis + (dataCopy1.pose.position.y - data.pose.position.y) * (dataCopy1.pose.position.y - data.pose.position.y)
dis = dis + (dataCopy1.pose.position.z - data.pose.position.z) * (dataCopy1.pose.position.z - data.pose.position.z)
if (dis < 0.008):
pass
else:
newRotMatrix = tf.transformations.euler_matrix(0, -1.57, 0)
transformedMatrix = numpy.dot(tMatrix, newRotMatrix)
q = tf.transformations.quaternion_from_matrix(transformedMatrix)
data.pose.orientation.x = q[0]
data.pose.orientation.y = q[1]
data.pose.orientation.z = q[2]
data.pose.orientation.w = q[3]
pub1.publish(data)
#rospy.sleep(5)
arm.set_joint_value_target(data, False)
arm.set_planner_id("RRTConnectkConfigDefault")
print "======= Waiting while setting joint value target"
#rospy.sleep(5)
#modifed from here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
# find who throw motion planer failures
try:
plan1 = arm.plan()
except Exception, e:
print "Exception thrown while path planning for grasp " + str(i)
print traceback.format_exc()
continue
print "============ Waiting while RVIZ displays plan1..."
#rospy.sleep(5)
a = raw_input("Shall i execute grasp" + str(i) + "? Say y/n ")
if a == "y":
print "executing"
arm.execute(plan1)
#########################
## Wait for Plan1 to finish
#########################
while(not IsMotionFinished(True)):
print "Plan1 is being executed!"
print "Plan1 has finished!!"
#b = raw_input("Shall i plan ik path" + str(i) + "? Say y/n ")
#if b == 'y' :
rospy.sleep(1)
print "============ Computing cartesian path..."
waypoints = []
waypoints.append(data.pose)
data.header.stamp = rospy.Time(0)
dataHandLink = tl.transformPose("Hand_Link", data)
dataHandLink.pose.position.z= dataHandLink.pose.position.z + x_offset
dataHandLink.header.stamp = rospy.Time(0)
wpose = tl.transformPose(arm.get_pose_reference_frame(), dataHandLink).pose
waypoints.append(copy.deepcopy(wpose))
print wpose
(plan2, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0, False) # jump_threshold
print fraction
print "============ Visualizing plan1 and plan2"
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory.trajectory.append(plan2)
display_trajectory_publisher.publish(display_trajectory);
#c = raw_input("Shall i execute ik path" + str(i) + "? Say y/n ")
#if c == 'y' :
#print "executing"
arm.execute(plan2)
#########################
## Wait for Plan2 to finish
#########################
while(not IsMotionFinished(True)):
print "Plan2 is being executed!"
print "Plan2 has finished!!"
#d=raw_input("Shall I close the gripper? Say y/n ")
#if d=='y':
# print "closing the gripper!"
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
strr = "close"
pub_gripper.publish(strr)
r.sleep()
break
if a == "n":
print "not executing"
continue
except Exception, err:
print "NO IK solution for grasp " + str(i)
print traceback.format_exc()
continue
def _disp_trj(self):
disp_trj = DisplayTrajectory()
disp_trj.trajectory_start = self._robot.get_current_state()
disp_trj.trajectory.append(self._arm_plan)
self._pub_traj.publish(disp_trj)
# Display the Trajectory
start_state = JointState()
start_state.header = Header()
start_state.header.stamp = rospy.Time.now()
start_state.name = plan.joint_trajectory.joint_names[:]
start_state.position = plan.joint_trajectory.points[-1].positions[:]
moveit_start_state = RobotState()
<<<<<<< HEAD
moveit_start_state.joint_state = start_state
=======
moveit_start_state.joint_state = start_state
>>>>>>> d95c7577c838d4cc104407dcb9fe5ba08262d4e9
pub_display_msg = DisplayTrajectory()
pub_display_msg.model_id = "sia5"
pub_display_msg.trajectory.append(plan)
pub_display_msg.trajectory_start = moveit_start_state
self.display_hp_pub.publish(pub_display_msg)
# Send Action and Wait result
goal = FollowJointTrajectoryGoal(trajectory=plan.joint_trajectory)
rospy.loginfo("Start Task")
self.client.send_goal(goal)
self.client.wait_for_result()
# Grasping
if self.grasp_[0] != self.grasp_[1]:
self.executeGrasp(self.grasp_[0])
self.grasp_[1] = self.grasp_[0]
# Update the task queue
self.task_q.pop(0)
# 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 publish_plan_B(self, plan):
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_B.get_current_state()
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher_B.publish(display_trajectory)
def visualize_plan_rviz(self):
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self._robot.get_current_state()
display_trajectory.trajectory.append(self.plan)
self._display_trajectory_publisher.publish(display_trajectory)
rospy.sleep(5)
def main():
"""
Examples of how to run me:
python scripts/main.py --help <------This prints out all the help messages
and describes what each parameter is
python scripts/main.py -t 1 -ar 1 -c workspace -a left --log
python scripts/main.py -t 2 -ar 2 -c velocity -a left --log
python scripts/main.py -t 3 -ar 3 -c torque -a right --log
python scripts/main.py -t 1 -ar 4 5 --path_only --log
NOTE: If running with the --moveit flag, it makes no sense
to also choose your controller to be workspace, since moveit
cannot accept workspace trajectories. This script simply ignores
the controller selection if you specify both --moveit and
--controller_name workspace, so if you want to run with moveit
simply leave --controller_name as default.
You can also change the rate, timeout if you want
"""
parser = argparse.ArgumentParser()
parser.add_argument('-task',
'-t',
type=str,
default='line',
help='Options: line, circle, square. Default: line')
parser.add_argument('-ar_marker',
'-ar',
nargs='+',
help='Which AR marker to use. Default: 1')
parser.add_argument(
'-controller_name',
'-c',
type=str,
default='jointspace',
help='Options: workspace, jointspace, or torque. Default: jointspace')
parser.add_argument('-arm',
'-a',
type=str,
default='left',
help='Options: left, right. Default: left')
parser.add_argument('-rate',
type=int,
default=200,
help="""
This specifies how many ms between loops. It is important to use a rate
and not a regular while loop because you want the loop to refresh at a
constant rate, otherwise you would have to tune your PD parameters if
the loop runs slower / faster. Default: 200""")
parser.add_argument(
'-timeout',
type=int,
default=None,
help=
"""after how many seconds should the controller terminate if it hasn\'t already.
Default: None""")
parser.add_argument(
'-num_way',
type=int,
default=300,
help=
'How many waypoints for the :obj:`moveit_msgs.msg.RobotTrajectory`. Default: 300'
)
parser.add_argument(
'--moveit',
action='store_true',
help=
"""If you set this flag, moveit will take the path you plan and execute it on
the real robot""")
parser.add_argument('--log',
action='store_true',
help='plots controller performance')
args = parser.parse_args()
rospy.init_node('moveit_node')
# this is used for sending commands (velocity, torque, etc) to the robot
limb = baxter_interface.Limb(args.arm)
# this is used to get the dynamics (inertia matrix, manipulator jacobian, etc) from the robot
# in the current position, UNLESS you specify other joint angles. see the source code
# https://github.com/valmik/baxter_pykdl/blob/master/src/baxter_pykdl/baxter_pykdl.py
# for info on how to use each method
kin = baxter_kinematics(args.arm)
# ADD COMMENT EHRE
tag_pos = [lookup_tag(marker) for marker in args.ar_marker]
# Get an appropriate RobotTrajectory for the task (circular, linear, or square)
# If the controller is a workspace controller, this should return a trajectory where the
# positions and velocities are workspace positions and velocities. If the controller
# is a jointspace or torque controller, it should return a trajectory where the positions
# and velocities are the positions and velocities of each joint.
robot_trajectory = get_trajectory(args.task, limb, kin, tag_pos,
args.num_way, args.controller_name)
# This is a wrapper around MoveIt! for you to use. We use MoveIt! to go to the start position
# of the trajectory
planner = PathPlanner('{}_arm'.format(args.arm))
if args.controller_name == "workspace":
pose = create_pose_stamped_from_pos_quat(
robot_trajectory.joint_trajectory.points[0].positions,
[0, 1, 0, 0], 'base')
plan = planner.plan_to_pose(pose)
else:
plan = planner.plan_to_joint_pos(
robot_trajectory.joint_trajectory.points[0].positions)
planner.execute_plan(plan)
if args.moveit:
# LAB 1 PART A
# by publishing the trajectory to the move_group/display_planned_path topic, you should
# be able to view it in RViz. You will have to click the "loop animation" setting in
# the planned path section of MoveIt! in the menu on the left side of the screen.
pub = rospy.Publisher('move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_trajectory)
# disp_traj.trajectory_start = planner._group.get_current_joint_values()
disp_traj.trajectory_start = RobotState()
pub.publish(disp_traj)
try:
raw_input('Press <Enter> to execute the trajectory using MOVEIT')
except KeyboardInterrupt:
sys.exit()
# uses MoveIt! to execute the trajectory. make sure to view it in RViz before running this.
# the lines above will display the trajectory in RViz
planner.execute_plan(robot_trajectory)
else:
# LAB 1 PART B
controller = get_controller(args.controller_name, limb, kin)
try:
raw_input(
'Press <Enter> to execute the trajectory using YOUR OWN controller'
)
except KeyboardInterrupt:
sys.exit()
# execute the path using your own controller.
done = controller.execute_path(robot_trajectory,
rate=args.rate,
timeout=args.timeout,
log=args.log)
if not done:
print 'Failed to move to position'
sys.exit(0)
def main():
# Robot Node initialization
rospy.init_node('robot_init', anonymous=True)
# Visible -> on/off in the RVIZ environment
visible_object = False
rospy.set_param('object_visible', visible_object)
# Moveit Initialization
group = moveit_commander.MoveGroupCommander('manipulator')
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
# Reset robot positions (go to home position)
rospy.wait_for_service('/reset_robot')
reset_robot = rospy.ServiceProxy('/reset_robot', Trigger)
reset_robot.call()
rospy.sleep(0.5)
# Initialize the current position of the robot
w_pose_initial = rospy.wait_for_message('/current_tcp_pose',
geometry_msgs.msg.PoseStamped,
timeout=None)
position = [
w_pose_initial.pose.position.x, w_pose_initial.pose.position.y,
w_pose_initial.pose.position.z
]
orientation = [
w_pose_initial.pose.orientation.w, w_pose_initial.pose.orientation.x,
w_pose_initial.pose.orientation.y, w_pose_initial.pose.orientation.z
]
# Set the parameters of the Object (display -> environment)
rospy.set_param('object_pos', position)
# Setting parameters for planning
vel_scaling_f = 1.0
acc_scaling_f = 1.0
group.set_max_velocity_scaling_factor(vel_scaling_f)
group.set_max_acceleration_scaling_factor(acc_scaling_f)
# Planner -> OMPL (Default) or BiTRRTkConfigDefault
group.set_planner_id('OMPL')
# Joint, Cartesian_1, Cartesian_2 or None
mode = 'Cartesian_1'
if mode == 'Joint':
# Generate Joint Trajectory
plan = joint_trajectory(group, [1.57, -1.57, 1.57, -1.57, -1.57, 0.0])
elif mode == 'Cartesian_1':
# Generate Cartesian Trajectory (1)
plan = cartesian_trajectory_1(group, [
position[0], position[1] - 0.1, position[2], orientation[0],
orientation[1], orientation[2], orientation[3]
])
elif mode == 'Cartesian_2':
# Generate Cartesian Trajectory (2)
plan = cartesian_trajectory_2(group, [
position[0], position[1], position[2], orientation[0],
orientation[1], orientation[2], orientation[3]
], [0.05, 0.25, 0.30], vel_scaling_f, acc_scaling_f, 0.01, True)
if mode != 'None':
rospy.loginfo('Intermediate points on the robot trajectory: %f' %
len(plan.joint_trajectory.points))
# Show trajectory
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Execute the trajectory
group.execute(plan, wait=True)
rospy.sleep(0.5)
# Reset
group.stop()
group.clear_path_constraints()
group.clear_pose_targets()
plan = planner.plan_to_joint_pos(
robot_trajectory.joint_trajectory.points[0].positions)
planner.execute_plan(plan)
if args.moveit:
# LAB 1 PART A
# by publishing the trajectory to the move_group/display_planned_path topic, you should
# be able to view it in RViz. You will have to click the "loop animation" setting in
# the planned path section of MoveIt! in the menu on the left side of the screen.
pub = rospy.Publisher('move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_trajectory)
# disp_traj.trajectory_start = planner._group.get_current_joint_values()
disp_traj.trajectory_start = RobotState()
pub.publish(disp_traj)
try:
raw_input('Press <Enter> to execute the trajectory using MOVEIT')
except KeyboardInterrupt:
sys.exit()
# uses MoveIt! to execute the trajectory. make sure to view it in RViz before running this.
# the lines above will display the trajectory in RViz
#print('ROBOT TRAJECTORY')
#path_list = []
#print(robot_trajectory.joint_trajectory.points[0].positions)
#for p in robot_trajectory.joint_trajectory.points:
#print(p.positions)
#path_list.append(p.positions)
#path_list = np.array(path_list)
