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([
