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)