temp = state.copy()
temp.update(cmd)
state.update({s: subs(e, temp) for s, e in int_rules.items()})
diff_points.append((state[x1], state[y1], state[a1]))
# md_points.append((state[x2], state[y2], state[a2]))
# if qpb.equilibrium_reached():
# print('optimization ende early')
# break
full_diff_points.append(diff_points)
vis = ROSVisualizer('diff_drive_vis', 'world')
# md_p = [point3(x, y, 0) for x, y, _ in md_points]
# md_d = [vector3(cos(a), sin(a), 0) for _, _, a in md_points]
vis.begin_draw_cycle('paths')
# vis.draw_sphere('paths', goal, 0.02, r=0, b=1)
for n, diff_points in enumerate(full_diff_points):
diff_p = [point3(x, y, 0) + vector3((n / 3) * 0.5, (n % 3)* -0.5, 0) for x, y, _ in diff_points]
diff_d = [vector3(cos(a), sin(a), 0) for _, _, a in diff_points]
vis.draw_strip('paths', cm.eye(4), 0.02, diff_p)
#vis.draw_strip('paths', spw.eye(4), 0.02, md_p, r=0, g=1)
vis.render('paths')
rospy.sleep(0.3)
if __name__ == '__main__':
rospy.init_node('axis_angle_vis')
vis = ROSVisualizer('axis_vis', 'world')
az, ay = [Position(x) for x in 'ax az'.split(' ')]
frame_rpy = frame3_rpy(0, ay, az, point3(0,0,0))
state = {ay: 0, az: 0}
points = [point3(0,0,0) + get_rot_vector(frame_rpy.subs({ay: sin(v), az: cos(v)})) for v in [(3.14512 / 25) * x for x in range(51)]]
vis.begin_draw_cycle('points')
vis.draw_strip('points', se.eye(4), 0.03, points)
vis.render('points')
rospy.sleep(1)
timer = Time()
while not rospy.is_shutdown():
now = Time.now()
if (now - timer).to_sec() >= 0.02:
state[ay] = sin(now.to_sec())
state[az] = cos(now.to_sec())
frame = frame_rpy.subs(state)
axis = get_rot_vector(frame)