def configure_simulation(dt, enableGUI):
global jointTorques
# Load the robot for Pinocchio
solo = robots_loader.loadSolo(True)
solo.initViewer(loadModel=True)
# Start the client for PyBullet
if enableGUI:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT)
# p.GUI for graphical version
# p.DIRECT for non-graphical version
# Set gravity (disabled by default)
p.setGravity(0, 0, -9.81)
# Load horizontal plane for PyBullet
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
# Load the robot for PyBullet
robotStartPos = [0, 0, 0.35]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
p.setAdditionalSearchPath(
"/opt/openrobots/share/example-robot-data/robots/solo_description/robots"
)
robotId = p.loadURDF("solo.urdf", robotStartPos, robotStartOrientation)
# Set time step of the simulation
p.setTimeStep(dt)
# Disable default motor control for revolute joints
revoluteJointIndices = [0, 1, 3, 4, 6, 7, 9, 10]
p.setJointMotorControlArray(
robotId,
jointIndices=revoluteJointIndices,
controlMode=p.VELOCITY_CONTROL,
targetVelocities=[0.0 for m in revoluteJointIndices],
forces=[0.0 for m in revoluteJointIndices])
# Enable torque control for revolute joints
jointTorques = [0.0 for m in revoluteJointIndices]
p.setJointMotorControlArray(robotId,
revoluteJointIndices,
controlMode=p.TORQUE_CONTROL,
forces=jointTorques)
# Compute one step of simulation for initialization
p.stepSimulation()
return robotId, solo, revoluteJointIndices
def init_viewer(enable_viewer):
# loadSolo(False) to load solo12
# loadSolo(True) to load solo8
solo = robots_loader.loadSolo(False)
if enable_viewer:
solo.initDisplay(loadModel=True)
if ('viewer' in solo.viz.__dict__):
solo.viewer.gui.addFloor('world/floor')
solo.viewer.gui.setRefreshIsSynchronous(False)
"""offset = np.zeros((19, 1))
offset[5, 0] = 0.7071067811865475
offset[6, 0] = 0.7071067811865475 - 1.0
temp = solo.q0 + offset"""
solo.display(solo.q0)
pin.centerOfMass(solo.model, solo.data, solo.q0, np.zeros((18, 1)))
pin.updateFramePlacements(solo.model, solo.data)
pin.crba(solo.model, solo.data, solo.q0)
return solo
from pinocchio.utils import *
import robots_loader
import numpy as np
import pinocchio as pin
from scipy.optimize import fmin_bfgs, fmin_slsqp
from time import sleep
from IPython import embed
from numpy.linalg import pinv
solo = robots_loader.loadSolo()
solo.initDisplay(loadModel=True)
q = solo.q0
qdes = q.copy() #reference configuration
solo.display(q)
def normalize(quaternion):
norm_q = np.linalg.norm(quaternion)
#if (norm_q > 1):
assert (norm_q > 1e-6)
return quaternion / norm_q
### Random configuration
q[3] = np.random.uniform(-1, 1) / 10
q[4] = np.random.uniform(-1, 1) / 10
q[5] = np.random.uniform(-1, 1) / 10
normalize(q[3:7])
## Sort contacts points to get only one contact per foot ##
def getContactPoint(contactPoints):
for i in range(0, len(contactPoints)):
# There may be several contact points for each foot but only one of them as a non zero normal force
if (contactPoints[i][9] != 0):
return contactPoints[i]
return 0 # If it returns 0 then it means there is no contact point with a non zero normal force (should not happen)
########################################################################
########################### START OF SCRIPT ############################
########################################################################
# Load the robot for Pinocchio
solo = robots_loader.loadSolo(True)
solo.initDisplay(loadModel=True)
########################################################################
## Initialization for control with mouse
qdes = (solo.q0).copy()
try:
# open the connection
print("Opening connection to SpaceNav driver ...")
sp.open()
print("... connection established.")
# register the close function if no exception was raised
atexit.register(sp.close)