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()
ax2.set_ylabel('Angle [deg]')
ax2.legend()
### 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?
motor_viz_frame.append(
VisualizationFrame(N, motor_masscenter[i], motor_shape))
arm_center.append(Point('a_c[%u]' % (i, )))
arm_center[i].set_pos(body_masscenter, body_arm / 2. * motorloc[i])
arm_viz_frame.append(
VisualizationFrame(
'arm[%u]' % (i, ),
body_frame.orientnew('arm%u' % (i, ), 'Axis',
[arm_angles[i], body_frame.z]), arm_center[i],
arm_shape))
for i in range(4):
for j in range(2):
blade_tip_viz_frame.append(
VisualizationFrame(N, blade_tip[2 * i + j], blade_tip_shape))
blade_center.append(Point('p_c[%u]' % (2 * i + j, )))
blade_center[2 * i + j].set_pos(
motor_masscenter[i], blade_length / 2. * blade_frame[2 * i + j].y)
blade_viz_frame.append(
VisualizationFrame('blade[%u]' % (2 * i + j, ),
blade_frame[2 * i + j], blade_center[2 * i + j],
blade_shape))
scene = Scene(N, O)
scene.visualization_frames = [
body_viz_frame
] + arm_viz_frame + motor_viz_frame + blade_viz_frame
print scene.visualization_frames
scene.states_symbols = q_sym + u_sym
scene.states_trajectories = y
scene.constants = {}
scene.display()
r_hip_shape = Sphere(color='black', radius = 0.1)
r_ankle_shape = Sphere(color='black', radius = 0.1)
l_ankle_viz_frame = VisualizationFrame(inertial_frame, l_ankle, l_ankle_shape)
l_hip_viz_frame = VisualizationFrame(inertial_frame, l_hip, l_hip_shape)
r_hip_viz_frame = VisualizationFrame(inertial_frame, r_hip, r_hip_shape)
r_ankle_viz_frame = VisualizationFrame(inertial_frame, r_leg_mass_center, r_ankle_shape)
constants_dict = dict(zip(constants, numerical_constants))
l_leg_shape = Cylinder(radius = 0.08, length = constants_dict[l_leg_length], color = 'blue')
l_leg_viz_frame = VisualizationFrame('Left Leg', l_leg_frame, l_leg_mass_center, l_leg_shape)
body_shape = Cylinder(radius = 0.08, length = constants_dict[hip_width], color = 'blue')
body_viz_frame = VisualizationFrame('Body', body_frame, body_mass_center, body_shape)
r_leg_shape = Cylinder(radius = 0.08, length = constants_dict[l_leg_length], color = 'red')
r_leg_viz_frame = VisualizationFrame('Right Leg', r_leg_frame, r_leg_mass_center, r_leg_shape)
scene = Scene(inertial_frame, l_ankle)
scene.visualization_frames = [l_ankle_viz_frame,l_hip_viz_frame, r_hip_viz_frame, r_ankle_viz_frame, l_leg_viz_frame, body_viz_frame, r_leg_viz_frame]
scene.generate_visualization_json(coordinates + speeds, constants, y, numerical_constants)
scene.display()
j0_center.set_pos(joint0, j0_length / 2 * j0_frame.y)
j1_center.set_pos(joint1, j1_length / 2 * j1_frame.y)
j2_center.set_pos(joint2, j2_com_length * j2_frame.y)
constants_dict = dict(zip(constants, numerical_constants))
l0_shape = Cylinder(radius=0.025,
length=constants_dict[j0_length],
color='peachpuff')
l0_viz_frame = VisualizationFrame('Link 0', j0_frame, j0_center, l0_shape)
l1_shape = Cylinder(radius=0.025,
length=constants_dict[j1_length],
color='peachpuff')
l1_viz_frame = VisualizationFrame('Link 1', j1_frame, j1_center, l1_shape)
l2_shape = Cylinder(radius=0.025,
length=constants_dict[j2_com_length] * 2,
color='peachpuff')
l2_viz_frame = VisualizationFrame('Link 2', j2_frame, j2_center, l2_shape)
scene = Scene(inertial_frame, joint0)
#make list of frames we want in scene
scene.visualization_frames = [
j0_viz_frame, j1_viz_frame, j2_viz_frame, EE_viz_frame, l0_viz_frame,
l1_viz_frame, l2_viz_frame
]
scene.states_symbols = coordinates + speeds
scene.constants = constants_dict
scene.states_trajectories = y
scene.display()
upper_leg_center = Point('u_c')
torso_center = Point('t_c')
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)
constants_dict = dict(zip(constants, numerical_constants))
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)
#make list of frames we want in the scene
scene.visualization_frames = [ankle_viz_frame,
knee_viz_frame,
hip_viz_frame,
head_viz_frame,
lower_leg_viz_frame,
upper_leg_viz_frame,
torso_viz_frame]
scene.states_symbols = coordinates + speeds
scene.constants = constants_dict
scene.states_trajectories = y
scene.display()
#scene.display_ipython()
