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
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()
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
target_shape = Sphere(name='sphere{}'.format(i), radius=0.02, color='green')
target_right_leg = Point('target_right_leg')
target_right_leg.set_pos(origin, (x_des[2] * inertial_frame.x) +
(x_des[3] * inertial_frame.y))
viz_frames.append(
VisualizationFrame('target_frame_right', inertial_frame, target_right_leg,
target_shape))
## Now the visualization frames can be passed in to create a scene.
scene = Scene(inertial_frame, origin, *viz_frames)
# Provide the data to compute the trajectories of the visualization frames.
scene.constants = dict(zip(constants, numerical_constants))
scene.states_symbols = coordinates + speeds
scene.states_trajectories = y
scene.display()
#%% export to c header
t = symbols('t')
a0 = ankle_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(
inertial_frame)
k0 = knee_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(
inertial_frame)
hl = hip_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(
inertial_frame)
hc = hip_center.pos_from(origin).express(inertial_frame).simplify().to_matrix(
inertial_frame)
hr = hip_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(
viz_frames = []
for i, (link, particle) in enumerate(zip(links, particles)):
link_shape = Cylinder(name='cylinder{}'.format(i),
radius=link_radius,
length=link_length,
color='red')
viz_frames.append(VisualizationFrame('link_frame{}'.format(i), link,
link_shape))
particle_shape = Sphere(name='sphere{}'.format(i),
radius=particle_radius,
color='blue')
viz_frames.append(VisualizationFrame('particle_frame{}'.format(i),
link.frame,
particle,
particle_shape))
# Now the visualization frames can be passed in to create a scene.
scene = Scene(I, O, *viz_frames)
# Provide the data to compute the trajectories of the visualization frames.
scene.constants = dict(zip(param_syms, param_vals))
scene.states_symbols = q + u
scene.states_trajectories = state_trajectories
scene.display()
color='green')
target_right_leg = Point('target_right_leg')
target_right_leg.set_pos(origin, (x_des[2] * inertial_frame.x)+(x_des[3] * inertial_frame.y))
viz_frames.append(VisualizationFrame('target_frame_right',
inertial_frame,
target_right_leg,
target_shape))
## Now the visualization frames can be passed in to create a scene.
scene = Scene(inertial_frame, origin, *viz_frames)
# Provide the data to compute the trajectories of the visualization frames.
scene.constants = dict(zip(constants, numerical_constants))
scene.states_symbols = coordinates+speeds
scene.states_trajectories = y
scene.display()
#%% export to c header
t = symbols('t')
a0 = ankle_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
k0 = knee_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hl = hip_left.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hc = hip_center.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
hr = hip_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
k1 = knee_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
a1 = ankle_right.pos_from(origin).express(inertial_frame).simplify().to_matrix(inertial_frame)
[(c_name, c_code), (h_name, c_header)] = codegen(
[("Jacobian",J),
