y.velocities = vel #get acc acc = el[counter + 3].split(": ")[1][1:-1] acc = acc.split(", ") for i in range(0, len(acc)): acc[i] = float(acc[i]) y.accelerations = acc #get time st = el[counter + 6].split(": ")[1] et = el[counter + 7].split(": ")[1] y.time_from_start.secs = float(st) y.time_from_start.nsecs = float(et) x.joint_trajectory.points.append(y) counter = counter + 8 acHan.execute_plan(x) t.close() rospy.signal_shutdown("Done") os._exit(0) roscpp_shutdown()
plans.append(pos) names.append(name[5:]) acHan.Grasp(4, pos, 1.0) elif "Move" in name: joints = out[2][1:-2] joints = joints.split(",") for x in range(0, len(joints)): joints[x] = float(joints[x]) pl = acHan.plan(joints) plans.append(pl) names.append(name[4:]) acHan.execute_plan(pl) rospy.sleep(1) acHan.set_start() r = open("saved_plans.txt", "w") i = 0 for p in plans: r.write("PLAN : " + str(names[i]) + " >\n" + str(p) + "\n") i = i + 1 t.close() r.close() rospy.signal_shutdown("Done") os._exit(0)