def toggle_show_plan(self):
if not self.show_plan_toggled:
self.init_plan_handles = []
# self.cur_segment_handle = None
return
if motion_plan.nb_stances < 2:
return
path = [stance.zmp.pos for stance in motion_plan.stances]
color1 = (0.5, 0.3, 0.)
env = pymanoid.get_env()
self.init_plan_handles = [
env.drawlinelist(array(path), linewidth=2, colors=color1),
env.drawlinelist(array(path[1:]), linewidth=2, colors=color1)
]
def record_video():
window_recorder.record_video = True
camera_travel.on_tick(sim)
time.sleep(2)
sim.start()
if __name__ == "__main__":
seed(24)
pylab.ion()
pymanoid.init(set_viewer=False)
robot = RobotModel(download_if_needed=True)
robot.set_transparency(0.3)
env = pymanoid.get_env()
env.SetViewer('qtcoin')
pymanoid.env.set_default_background_color()
viewer = pymanoid.get_viewer()
viewer.SetCamera(
[[5.39066316e-01, 3.61154816e-01, -7.60903874e-01, 6.57677031e+00],
[8.42254221e-01, -2.26944015e-01, 4.88982864e-01, -2.9774201e+00],
[3.91593606e-03, -9.04468691e-01, -4.26522042e-01, 2.25269456e+00],
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
contacts = generate_contacts()
com = PointMass([0, 0, 0], robot.mass, visible=False)
free_foot = FreeFoot(color='c', visible=False)
preview_buffer = PreviewBuffer(com, free_foot)
fsm = StateMachine(robot,
contacts,
try:
# you can vary ``custom_mass`` to check that it has no effect
vertices = compute_static_polygon_cdd_only(contacts, custom_mass)
gui_handles['cdd_only'] = draw_com_polygon(vertices, black)
except Exception as e:
print "draw_cdd_only_thread:", e
continue
time.sleep(1e-2)
if __name__ == "__main__":
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])
except Exception as e:
print "draw_acceleration_thread:", e
continue
time.sleep(1e-2)
def recompute_static_polygon():
global static_handle
static_handle = draw_static_polygon(
contacts, p=[0., 0., robot.com[2]], combined='g.-#')
if __name__ == "__main__":
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)
import pymanoid
except ImportError:
import os
import sys
script_path = os.path.realpath(__file__)
sys.path.append(os.path.dirname(script_path) + '/../')
import pymanoid
from pymanoid.tasks import COMTask, ContactTask, DOFTask, PostureTask
if __name__ == '__main__':
pymanoid.init()
robot = pymanoid.robots.JVRC1('JVRC-1.dae', download_if_needed=True)
viewer = pymanoid.get_env().GetViewer()
viewer.SetCamera([
[-0.28985317, 0.40434422, -0.86746233, 2.73872042],
[0.95680251, 0.10095043, -0.2726499, 0.86080128],
[-0.02267371, -0.90901857, -0.41613837, 2.06654644],
[0., 0., 0., 1.]])
# Initial robot pose
robot.set_transparency(0.4)
dof_targets = [ # will also be passed to the IK
(robot.R_SHOULDER_R, -.5),
(robot.L_SHOULDER_R, +.5)]
q_init = robot.q.copy()
for (dof_id, dof_ref) in dof_targets:
robot.set_dof_values([dof_ref], [dof_id])
q_init[dof_id] = dof_ref