def visualize(): # Create shapes cube_1_shape = Cube(color='darkblue', length = 0.05) cube_2_shape = Cube(color='darkred', length = 0.05) # create frames to attach to shapes cube_1_frame = VisualizationFrame(I1, p1, cube_1_shape) cube_2_frame = VisualizationFrame(I2, p2, cube_2_shape) # create scene with a frame base and origin scene = Scene(N,N_o) # append frames that should be visualized scene.visualization_frames = [cube_1_frame, cube_2_frame] # provide constants constants_dict = dict(zip(constants, numerical_constants)) # give symbolic states and their values scene.states_symbols = coordinates + speeds scene.constants = constants_dict scene.states_trajectories = y #visualize scene.frames_per_second=800 # default is 30, which is very slow scene.display()
### Visualization # Create shapes cube_i_shape = Cube(color='darkblue', length=0.05) cube_j_shape = Cube(color='darkred', length=0.05) # create frames to attach to shapes cube_i_frame = VisualizationFrame(I, p_i, cube_i_shape) cube_j_frame = VisualizationFrame(J, p_j, cube_j_shape) # create scene with a frame base and origin scene = Scene(N, N_o) # append frames that should be visualized scene.visualization_frames = [cube_i_frame, cube_j_frame] # privide constants constants_dict = dict(zip(constants, numerical_constants)) # give symbolic states and their values scene.states_symbols = coordinates + speeds scene.constants = constants_dict scene.states_trajectories = y #visualize scene.frames_per_second = 1800 # default is 30, which is very slow #scene.display() # TODO Create points and forces at correct locations and catching sides,rather than having coil at CoM # TODO Control theory - NOTE the same Human Tutorial includes a Control part, but try using PD or inverse model control, then making optimal controller e.g. h infinity- maybe for arbitrary metamodule shapes? # print(N.dcm(I)) #DCM: format is N.xyz = N.dcm(I) * I.xyz
lower_leg_center.set_pos(ankle, lower_leg_length / 2 * lower_leg_frame.y) upper_leg_center.set_pos(knee, upper_leg_length / 2 * upper_leg_frame.y) torso_center.set_pos(hip, torso_com_length * torso_frame.y) lower_leg_shape = Cylinder(radius=0.08, length=constants_dict[lower_leg_length], color='blue') lower_leg_viz_frame = VisualizationFrame('Lower Leg', lower_leg_frame, lower_leg_center, lower_leg_shape) upper_leg_shape = Cylinder(radius=0.08, length=constants_dict[upper_leg_length], color='green') upper_leg_viz_frame = VisualizationFrame('Upper Leg', upper_leg_frame, upper_leg_center, upper_leg_shape) torso_shape = Cylinder(radius=0.08, length=2 * constants_dict[torso_com_length], color='red') torso_viz_frame = VisualizationFrame('Torso', torso_frame, torso_center, torso_shape) scene = Scene(inertial_frame, ankle, ankle_viz_frame, knee_viz_frame, hip_viz_frame, head_viz_frame, lower_leg_viz_frame, upper_leg_viz_frame, torso_viz_frame) scene.constants = constants_dict scene.states_symbols = coordinates + speeds scene.states_trajectories = y