def move_com_back_and_forth(duration, dt=1e-2):
for t in numpy.arange(0, duration, dt):
loop_start = time.time()
com_var = numpy.sin(t) * numpy.array([.2, 0, 0])
com.set_pos(init_com + numpy.array([-0.2, 0., 0.]) + com_var)
robot.step_ik(dt)
rem_time = dt - (time.time() - loop_start)
if rem_time > 0:
time.sleep(rem_time)
if __name__ == '__main__':
sim = pymanoid.Simulation(dt=0.03)
robot = JVRC1('JVRC-1.dae', download_if_needed=True)
sim.set_viewer()
sim.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])
Instance of the current simulation.
"""
self.com_target.set_accel(self.preview_buffer.cur_control)
class PointMassWrenchDrawer(PointMassWrenchDrawer):
def on_tick(self, sim):
self.contact_set = fsm.cur_stance
super(PointMassWrenchDrawer, self).on_tick(sim)
if __name__ == "__main__":
seed(42)
sim = pymanoid.Simulation(dt=0.03)
robot = JVRC1(download_if_needed=True)
sim.set_viewer()
robot.set_transparency(0.3)
staircase = generate_staircase(
radius=1.4,
angular_step=0.5,
height=1.2,
roughness=0.5,
friction=0.7,
ds_duration=0.7,
ss_duration=1.0,
init_com_offset=array([0., 0., 0.]))
com_target = PointMass([0, 0, 0], 20.)
preview_buffer = PreviewBuffer(
def set_camera_2():
global cam_id
pymanoid.get_viewer().SetCamera([[0., -1., 0., 0.7], [-1., 0., 0., 0.3],
[0., 0., -1., 4.65], [0., 0., 0., 1.]])
cam_id = 2
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()