<s> from __future__ import division <EOL> import openravepy <EOL> import numpy as np <EOL> from lfd . environment . simulation import DynamicSimulation <EOL> from lfd . environment . simulation_object import XmlSimulationObject , BoxSimulationObject , RopeSimulationObject , CylinderSimulationObject <EOL> from lfd . environment import sim_util <EOL> table_height = <NUM_LIT> <EOL> helix_ang0 = <NUM_LIT:0> <EOL> helix_ang1 = <NUM_LIT:4> * np . pi <EOL> helix_radius = <NUM_LIT> <EOL> helix_center = np . r_ [ <NUM_LIT> , <NUM_LIT:0> ] <EOL> helix_height0 = table_height + <NUM_LIT> <EOL> helix_height1 = table_height + <NUM_LIT> + <NUM_LIT> <EOL> helix_length = np . linalg . norm ( np . r_ [ ( helix_ang1 - helix_ang0 ) * helix_radius , helix_height1 - helix_height0 ] ) <EOL> num = np . round ( helix_length / <NUM_LIT> ) <EOL> helix_angs = np . linspace ( helix_ang0 , helix_ang1 , num ) <EOL> helix_heights = np . linspace ( helix_height0 , helix_height1 , num ) <EOL> init_rope_nodes = np . c_ [ helix_center + helix_radius * np . c_ [ np . cos ( helix_angs ) , np . sin ( helix_angs ) ] , helix_heights ] <EOL> rope_params = sim_util . RopeParams ( ) <EOL> cyl_radius = <NUM_LIT> <EOL> cyl_height = <NUM_LIT> <EOL> cyl_pos0 = np . r_ [ <NUM_LIT> , helix_radius , table_height + <NUM_LIT> ] <EOL> cyl_pos1 = np . r_ [ <NUM_LIT> , - helix_radius , table_height + <NUM_LIT> ] <EOL> sim_objs = [ ] <EOL> sim_objs . append ( XmlSimulationObject ( "<STR_LIT>" , dynamic = False ) ) <EOL> sim_objs . append ( BoxSimulationObject ( "<STR_LIT>" , [ <NUM_LIT:1> , <NUM_LIT:0> , table_height - <NUM_LIT> ] , [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] , dynamic = False ) ) <EOL> sim_objs . append ( RopeSimulationObject ( "<STR_LIT>" , init_rope_nodes , rope_params ) ) <EOL> sim_objs . append ( CylinderSimulationObject ( "<STR_LIT>" , cyl_pos0 , cyl_radius , cyl_height , dynamic = True ) ) <EOL> sim_objs . append ( CylinderSimulationObject ( "<STR_LIT>" , cyl_pos1 , cyl_radius , cyl_height , dynamic = True ) ) <EOL> sim = DynamicSimulation ( ) <EOL> sim . add_objects ( sim_objs ) <EOL> sim . create_viewer ( ) <EOL> sim . robot . SetDOFValues ( [ <NUM_LIT> ] , [ sim . robot . GetJoint ( '<STR_LIT>' ) . GetJointIndex ( ) ] ) <EOL> sim_util . reset_arms_to_side ( sim ) <EOL> for i in range ( <NUM_LIT:2> ) : <EOL> bt_cyl = sim . bt_env . GetObjectByName ( '<STR_LIT>' % i ) <EOL> T = openravepy . matrixFromAxisAngle ( np . array ( [ np . pi / <NUM_LIT:2> , <NUM_LIT:0> , <NUM_LIT:0> ] ) ) <EOL> T [ : <NUM_LIT:3> , <NUM_LIT:3> ] = bt_cyl . GetTransform ( ) [ : <NUM_LIT:3> , <NUM_LIT:3> ] <EOL> bt_cyl . SetTransform ( T ) <EOL> sim . update ( ) <EOL> sim . settle ( max_steps = <NUM_LIT:1000> ) <EOL> sim . viewer . Idle ( ) </s>
