cassietest.py

#!/usr/bin/env python3

# Copyright (c) 2018 Dynamic Robotics Laboratory
#
# Permission to use, copy, modify, and distribute this software for any
# purpose with or without fee is hereby granted, provided that the above
# copyright notice and this permission notice appear in all copies.
#
# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

from cassiemujoco import *
import time
import numpy as np
import math

# Initialize cassie simulation
sim = CassieSim("../model/cassie.xml")
vis = CassieVis(sim)

# Set control parameters
u = pd_in_t()
for i in range(5):
    u.leftLeg.motorPd.torque[i] = 0
    u.rightLeg.motorPd.torque[i] = 0
    u.leftLeg.motorPd.dTarget[i] = 0
    u.rightLeg.motorPd.dTarget[i] = 0
    u.leftLeg.motorPd.pGain[i] = 0
    u.rightLeg.motorPd.pGain[i] = 0
    u.leftLeg.motorPd.dGain[i] = 0
    u.rightLeg.motorPd.dGain[i] = 0

    u.leftLeg.motorPd.pTarget[i] = 0
    u.rightLeg.motorPd.pTarget[i] = 0

# Record time
t = time.monotonic()

# Run until window is closed or vis is quit
draw_state = vis.draw(sim)
while draw_state:# and draw_state2:
    if not vis.ispaused():
        for i in range(60):
            y = sim.step_pd(u)

    draw_state = vis.draw(sim)

    while time.monotonic() - t < 60*0.0005:
        time.sleep(0.0001)
    t = time.monotonic()