def __init__(self, robot, visible=True): self.arrow = None self.com = pymanoid.Cube(0.01, robot.com, name='Pendulum-COM', color='b') self.com.pd = zeros(3) self.zmp = pymanoid.Cube(0.015, self.com.pos + array([0, 0, 1]), name='Pendulum-ZMP', color='b', visible=True) self.is_visible = visible
def __init__(self, motion_plan, stance_id, stance_dict): contact_dict = {} for (link, val) in stance_dict['contacts'].iteritems(): if type(val) is str: contact_dict[link] = motion_plan.contacts[val] else: # c is a dict with 'pos' and 'rpy' keys contact_dict[link] = motion_plan.create_contact( pos=val['pos'], rpy=val['rpy'], name='Stance-%d-%s' % (stance_id, link), visible=False) free_dict = {} if 'free' in stance_dict: for (link, val) in stance_dict['free'].iteritems(): if type(val) is str: free_dict[link] = motion_plan.contacts[val] else: # c is a dict with 'pos' and 'rpy' keys free_dict[link] = motion_plan.create_contact( pos=val['pos'], rpy=val['rpy'], name='Stance-%d-Free-%s' % (stance_id, link), color=TARGET_COLOR, visible=False) free_dict[link].set_transparency(0.5) if motion_plan.zmp_height is not None: # when defined, the zmp_height field overrides stance ZMP's stance_dict['zmp'][2] = motion_plan.zmp_height self.contacts = pymanoid.ContactSet(contact_dict) self.dict_repr = stance_dict self.free = pymanoid.ContactSet(free_dict) self.stance_id = stance_id self.step_duration = stance_dict['step_duration'] self.zmp = pymanoid.Cube(0.02, stance_dict['zmp'], color=TARGET_COLOR, name='Stance-%d-ZMP' % stance_id, visible=False)
self.com_above = com_above self.stance = stance def on_tick(self, sim): self.stance.com.set_x(self.com_above.x) self.stance.com.set_y(self.com_above.y) if __name__ == "__main__": sim = pymanoid.Simulation(dt=0.03) robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True) sim.set_viewer() sim.set_camera_top(x=0., y=0., z=3.) robot.set_transparency(0.25) com_above = pymanoid.Cube(0.02, [0.05, 0.04, z_polygon], color='b') stance = Stance.from_json('../stances/double.json') stance.com.hide() stance.bind(robot) robot.ik.solve() method = "hull" if "bretl" in sys.argv: method = "bretl" elif "cdd" in sys.argv: method = "cdd" com_sync = COMSync(stance, com_above) polygon_drawer = SupportPolygonDrawer(stance, method) wrench_drawer = StaticWrenchDrawer(stance)
pymanoid.init() robot = RobotModel(download_if_needed=True) robot.set_transparency(0.2) robot_mass = robot.mass viewer = pymanoid.get_env().GetViewer() viewer.SetCamera( numpy.array([[0.60587192, -0.36596244, 0.70639274, -2.4904027], [-0.79126787, -0.36933163, 0.48732874, -1.6965636], [0.08254916, -0.85420468, -0.51334199, 2.79584694], [0., 0., 0., 1.]])) viewer.SetBkgndColor([1, 1, 1]) fname = sys.argv[1] if len(sys.argv) > 1 else '../stances/triple.json' contacts = pymanoid.ContactSet.from_json(fname) com_target = pymanoid.Cube(0.01, visible=True) outbox = pymanoid.Cube(0.02, color='b') if 'single.json' in fname: outbox.set_pos([0., 0., z_high]) elif 'double.json' in fname or 'triple.json' in fname: outbox.set_pos([0.3, 0.04, z_high]) else: warn("Unknown contact set, you will have to set the COM position.") with robot_lock: robot.set_dof_values(robot.q_halfsit) robot.set_active_dofs(robot.chest + robot.legs + robot.arms + robot.free) robot.init_ik() robot.generate_posture(contacts)
pymanoid.init(set_viewer=False) robot = RobotModel(download_if_needed=True) pymanoid.get_env().SetViewer('qtcoin') viewer = pymanoid.get_viewer() viewer.SetCamera(array([ [0.60587192, -0.36596244, 0.70639274, -2.4904027], [-0.79126787, -0.36933163, 0.48732874, -1.6965636], [0.08254916, -0.85420468, -0.51334199, 2.79584694], [0., 0., 0., 1.]])) robot.set_transparency(0.5) robot_mass = robot.mass fname = sys.argv[1] if len(sys.argv) > 1 else '../stances/double.json' contacts = pymanoid.ContactSet.from_json(fname) com = pymanoid.Cube(0.01, color='r') outbox = pymanoid.Cube(0.05, color='g') com_target = pymanoid.Cube(0.01, visible=False) if 'single.json' in fname: outbox.set_pos([0., 0., z_high]) elif 'double.json' in fname or 'triple.json' in fname: outbox.set_pos([0.3, 0.14, z_high]) com_target.set_x(outbox.x) com_target.set_y(outbox.y) com_target.set_z(outbox.z - z_high + z_mid) robot.set_dof_values(robot.q_halfsit) robot.set_active_dofs( robot.chest + robot.free + robot.legs + robot.arms) robot.init_ik() robot.generate_posture(contacts)
if __name__ == '__main__': init_ros() pymanoid.init('editor/jvrc1_env.xml') env = pymanoid.get_env() collision_handle = env.RegisterCollisionCallback(collision_callback) viewer = env.GetViewer() motion_plan = MotionPlan(show_targets=gui_defaults['show_targets']) robot = JVRC1() robot.set_transparency(0.) customize_joint_limits(robot) pendulum = Pendulum(robot) com_ghost = pymanoid.Cube(0.01, robot.com, color='g', visible=motion_plan.show_targets) com_real = pymanoid.Cube(0.01, robot.com, color='r', visible=True) zmp_real = pymanoid.Cube(0.005, pendulum.zmp.pos, color='r', visible=True) if '-c1' in sys.argv: set_camera_1() elif '-c2' in sys.argv: set_camera_2() else: # default camera set_camera_0() if '-r' in sys.argv or '--record' in sys.argv: from re import search record_screen = True print "Preparing for screen recording..."
sim = pymanoid.Simulation(dt=0.03) robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True) sim.set_viewer() sim.viewer.SetCamera([[0.60587192, -0.36596244, 0.70639274, -2.4904027], [-0.79126787, -0.36933163, 0.48732874, -1.6965636], [0.08254916, -0.85420468, -0.51334199, 2.79584694], [0., 0., 0., 1.]]) robot.set_transparency(0.25) com_height = 0.9 # [m] z_polygon = 2. # [m] com_target = PointMass(pos=[0., 0., com_height], mass=robot.mass, color='b', visible=False) com_above = pymanoid.Cube(0.02, [0.05, 0.04, 2.], color='b') stance = Stance(com=com_target, left_foot=Contact(shape=robot.sole_shape, pos=[0.20, 0.15, 0.1], rpy=[0.4, 0, 0], static_friction=0.5, visible=True), right_foot=Contact(shape=robot.sole_shape, pos=[-0.2, -0.195, 0.], rpy=[-0.4, 0, 0], static_friction=0.5, visible=True)) robot.init_ik(robot.whole_body) robot.set_dof_values([