bb87928bce
Upstream: https://github.com/Rhoban/onshape-to-robot Upstream-Commit: 80e710700aac9573a2230f74f7ce9e094833a0bc Upstream-Branch: master
542 lines
18 KiB
Python
542 lines
18 KiB
Python
from transforms3d.quaternions import mat2quat, quat2mat
|
|
import math
|
|
import sys
|
|
import time
|
|
import numpy as np
|
|
import pybullet as p
|
|
from time import sleep
|
|
import os
|
|
import re
|
|
|
|
|
|
class Simulation:
|
|
"""
|
|
A Bullet simulation involving Onshape to robot model
|
|
"""
|
|
|
|
def __init__(self, robotPath, floor=True, fixed=False, transparent=False, gui=True, ignore_self_collisions=False,
|
|
realTime=True, panels=False, useUrdfInertia=True, dt=0.002, physicsClient = None):
|
|
"""Creates an instance of humanoid simulation
|
|
|
|
Keyword Arguments:
|
|
field {bool} -- enable the display of the field (default: {False})
|
|
fixed {bool} -- makes the base of the robot floating/fixed (default: {False})
|
|
transparent {bool} -- makes the robot transparent (default: {False})
|
|
gui {bool} -- enables the gui visualizer, if False it will runs headless (default {True})
|
|
realTime {bool} -- try to have simulation in real time (default {True})
|
|
panels {bool} -- show/hide the user interaction pyBullet panels (default {False})
|
|
useUrdfInertia {bool} -- use URDF from URDF file (default {True})
|
|
dt {float} -- time step (default {0.002})
|
|
"""
|
|
|
|
self.dir = os.path.dirname(os.path.abspath(__file__))
|
|
self.gui = gui
|
|
self.realTime = realTime
|
|
self.t = 0
|
|
self.start = time.time()
|
|
self.dt = dt
|
|
self.mass = None
|
|
|
|
# Debug lines drawing
|
|
self.lines = []
|
|
self.currentLine = 0
|
|
self.lastLinesDraw = 0
|
|
self.lineColors = [[1, 0, 0], [0, 1, 0], [
|
|
0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1]]
|
|
|
|
# Instanciating bullet
|
|
if physicsClient is None:
|
|
if gui:
|
|
physicsClient = p.connect(p.GUI)
|
|
else:
|
|
physicsClient = p.connect(p.DIRECT)
|
|
p.setGravity(0, 0, -9.81)
|
|
|
|
# Light GUI
|
|
if not panels:
|
|
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
|
|
p.configureDebugVisualizer(
|
|
p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
|
|
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
|
|
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
|
|
|
|
p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 1)
|
|
|
|
# Loading floor and/or plane ground
|
|
if floor:
|
|
self.floor = p.loadURDF(self.dir+'/bullet/plane.urdf')
|
|
else:
|
|
self.floor = None
|
|
|
|
# Loading robot
|
|
startPos = [0, 0, 0]
|
|
if not fixed:
|
|
startPos[2] = 1
|
|
startOrientation = p.getQuaternionFromEuler([0, 0, 0])
|
|
flags = 0 if ignore_self_collisions else p.URDF_USE_SELF_COLLISION
|
|
if useUrdfInertia:
|
|
flags += p.URDF_USE_INERTIA_FROM_FILE
|
|
self.robot = p.loadURDF(robotPath,
|
|
startPos, startOrientation,
|
|
flags=flags, useFixedBase=fixed)
|
|
|
|
# Setting frictions parameters to default ones
|
|
self.setFloorFrictions()
|
|
|
|
# Engine parameters
|
|
p.setPhysicsEngineParameter(fixedTimeStep=self.dt, maxNumCmdPer1ms=0)
|
|
# p.setRealTimeSimulation(0)
|
|
# p.setPhysicsEngineParameter(numSubSteps=1)
|
|
|
|
# Retrieving joints and frames
|
|
self.joints = {}
|
|
self.passive_joints = {}
|
|
self.jointsInfos = {}
|
|
self.jointsIndexes = {}
|
|
self.frames = {}
|
|
self.maxTorques = {}
|
|
|
|
# Collecting the available joints
|
|
n = 0
|
|
for k in range(p.getNumJoints(self.robot)):
|
|
jointInfo = p.getJointInfo(self.robot, k)
|
|
name = jointInfo[1].decode('utf-8')
|
|
|
|
if 'passive' in name:
|
|
self.passive_joints[name] = k
|
|
elif not name.endswith('_fixed'):
|
|
if '_frame' in name:
|
|
self.frames[name] = k
|
|
else:
|
|
self.jointsIndexes[name] = n
|
|
n += 1
|
|
self.joints[name] = k
|
|
self.jointsInfos[name] = {
|
|
'type': jointInfo[2]
|
|
}
|
|
if jointInfo[8] < jointInfo[9]:
|
|
self.jointsInfos[name]['lowerLimit'] = jointInfo[8]
|
|
self.jointsInfos[name]['upperLimit'] = jointInfo[9]
|
|
|
|
# Changing robot opacity if transparent set to true
|
|
if transparent:
|
|
for k in range(p.getNumJoints(self.robot)):
|
|
p.changeVisualShape(self.robot, k, rgbaColor=[
|
|
0.3, 0.3, 0.3, 0.3])
|
|
|
|
print('* Found '+str(len(self.joints))+' DOFs')
|
|
print('* Found '+str(len(self.frames))+' frames')
|
|
|
|
def setFloorFrictions(self, lateral=1, spinning=-1, rolling=-1):
|
|
"""Sets the frictions with the plane object
|
|
|
|
Keyword Arguments:
|
|
lateral {float} -- lateral friction (default: {1.0})
|
|
spinning {float} -- spinning friction (default: {-1.0})
|
|
rolling {float} -- rolling friction (default: {-1.0})
|
|
"""
|
|
if self.floor is not None:
|
|
p.changeDynamics(self.floor, -1, lateralFriction=lateral,
|
|
spinningFriction=spinning, rollingFriction=rolling)
|
|
|
|
def lookAt(self, target):
|
|
"""Control the look of the visualizer camera
|
|
|
|
Arguments:
|
|
target {tuple} -- target as (x,y,z) tuple
|
|
"""
|
|
if self.gui:
|
|
params = p.getDebugVisualizerCamera()
|
|
p.resetDebugVisualizerCamera(
|
|
params[10], params[8], params[9], target)
|
|
|
|
def getRobotPose(self):
|
|
"""Gets the robot (origin) position
|
|
|
|
Returns:
|
|
(tuple(3), tuple(3)) -- (x,y,z), (roll, pitch, yaw)
|
|
"""
|
|
pose = p.getBasePositionAndOrientation(self.robot)
|
|
return (pose[0], p.getEulerFromQuaternion(pose[1]))
|
|
|
|
def frameToWorldMatrix(self, frame):
|
|
"""Gets the given frame to world matrix transformation. can be a frame name
|
|
from URDF/SDF or "origin" for the part origin
|
|
|
|
Arguments:
|
|
frame {str} -- frame name
|
|
|
|
Returns:
|
|
np.matrix -- a 4x4 matrix
|
|
"""
|
|
|
|
if frame == 'origin':
|
|
frameToWorldPose = p.getBasePositionAndOrientation(self.robot)
|
|
else:
|
|
frameToWorldPose = p.getLinkState(self.robot, self.frames[frame])
|
|
|
|
return self.poseToMatrix(frameToWorldPose)
|
|
|
|
def transformation(self, frameA, frameB):
|
|
"""Transformation matrix AtoB
|
|
|
|
Arguments:
|
|
frameA {str} -- frame A name
|
|
frameB {str} -- frame B name
|
|
|
|
Returns:
|
|
np.matrix -- A 4x4 matrix
|
|
"""
|
|
AtoWorld = self.frameToWorldMatrix(frameA)
|
|
BtoWorld = self.frameToWorldMatrix(frameB)
|
|
|
|
return np.linalg.inv(BtoWorld) * AtoWorld
|
|
|
|
def poseToMatrix(self, pose):
|
|
"""Converts a pyBullet pose to a transformation matrix"""
|
|
translation = pose[0]
|
|
quaternion = pose[1]
|
|
|
|
# NOTE: PyBullet quaternions are x, y, z, w
|
|
rotation = quat2mat([quaternion[3], quaternion[0],
|
|
quaternion[1], quaternion[2]])
|
|
|
|
m = np.identity(4)
|
|
m[0:3, 0:3] = rotation
|
|
m.T[3, 0:3] = translation
|
|
|
|
return np.matrix(m)
|
|
|
|
def matrixToPose(self, matrix):
|
|
"""Converts a transformation matrix to a pyBullet pose"""
|
|
arr = np.array(matrix)
|
|
translation = list(arr.T[3, 0:3])
|
|
quaternion = mat2quat(arr[0:3, 0:3])
|
|
|
|
# NOTE: PyBullet quaternions are x, y, z, w
|
|
quaternion = [quaternion[1], quaternion[2],
|
|
quaternion[3], quaternion[0]]
|
|
|
|
return translation, quaternion
|
|
|
|
def setRobotPose(self, pos, orn):
|
|
"""Sets the robot (origin) pose
|
|
|
|
Arguments:
|
|
pos {tuple} -- (x,y,z) position
|
|
orn {tuple} -- (x,y,z,w) quaternions
|
|
"""
|
|
p.resetBasePositionAndOrientation(self.robot, pos, orn)
|
|
|
|
def reset(self, height=0.5, orientation='straight'):
|
|
"""Resets the robot for experiment (joints, robot position, simulator time)
|
|
|
|
Keyword Arguments:
|
|
height {float} -- height of the reset (m) (default: {0.55})
|
|
orientation {str} -- orientation (straight, front or back) of the robot (default: {'straight'})
|
|
"""
|
|
self.lines = []
|
|
self.t = 0
|
|
self.start = time.time()
|
|
|
|
# Resets the robot position
|
|
orn = [0, 0, 0]
|
|
if orientation == 'front':
|
|
orn = [0, math.pi/2, 0]
|
|
elif orientation == 'back':
|
|
orn = [0, -math.pi/2, 0]
|
|
self.resetPose([0, 0, height], p.getQuaternionFromEuler(orn))
|
|
|
|
# Reset the joints to 0
|
|
for entry in self.joints.values():
|
|
p.resetJointState(self.robot, entry, 0)
|
|
|
|
def resetPose(self, pos, orn):
|
|
"""Called by reset() with the robot pose
|
|
|
|
Arguments:
|
|
pos {tuple} -- (x,y,z) position
|
|
orn {tuple} -- (x,y,z,w) quaternions
|
|
"""
|
|
self.setRobotPose(pos, orn)
|
|
|
|
def getFrame(self, frame):
|
|
"""Gets the given frame
|
|
|
|
Arguments:
|
|
frame {str} -- frame name
|
|
|
|
Returns:
|
|
tuple -- (pos, orn), where pos is (x, y, z) and orn is quaternions (x, y, z, w)
|
|
"""
|
|
jointState = p.getLinkState(self.robot, self.frames[frame])
|
|
return (jointState[0], jointState[1])
|
|
|
|
def getFrames(self):
|
|
"""Gets the available frames in the current robot model
|
|
|
|
Returns:
|
|
dict -- dict of str -> (pos, orientation)
|
|
"""
|
|
frames = {}
|
|
|
|
for name in self.frames.keys():
|
|
jointState = p.getLinkState(self.robot, self.frames[name])
|
|
pos = jointState[0]
|
|
orientation = p.getEulerFromQuaternion(jointState[1])
|
|
frames[name] = [pos, orientation]
|
|
|
|
return frames
|
|
|
|
def getVelocity(self, frame):
|
|
"""Gets the velocity of the given frame
|
|
|
|
Arguments:
|
|
frame {str} -- frame name
|
|
|
|
Returns:
|
|
tuple -- (linear, angular)
|
|
"""
|
|
jointState = p.getLinkState(self.robot, self.frames[frame], computeLinkVelocity=True)
|
|
return (jointState[6], jointState[7])
|
|
|
|
def resetJoints(self, joints):
|
|
"""Reset all the joints to a given position
|
|
|
|
Arguments:
|
|
joints {dict} -- dict of joint name -> angle (float, radian)
|
|
"""
|
|
for name in joints:
|
|
p.resetJointState(self.robot, self.joints[name], joints[name])
|
|
|
|
def setJoints(self, joints):
|
|
"""Set joint targets for motor control in simulation
|
|
|
|
Arguments:
|
|
joints {dict} -- dict of joint name -> angle (float, radian)
|
|
|
|
Raises:
|
|
Exception: if a joint is not found, exception is raised
|
|
|
|
Returns:
|
|
applied {dict} -- dict of joint states (position, velocity, reaction forces, applied torque)
|
|
"""
|
|
applied = {}
|
|
|
|
for name in self.passive_joints:
|
|
p.setJointMotorControl2(self.robot, self.passive_joints[name], controlMode=p.VELOCITY_CONTROL, force=0)
|
|
|
|
for name in joints.keys():
|
|
if name in self.joints:
|
|
if name.endswith('_speed'):
|
|
p.setJointMotorControl2(
|
|
self.robot, self.joints[name], p.VELOCITY_CONTROL, targetVelocity=joints[name])
|
|
else:
|
|
if name in self.maxTorques:
|
|
maxTorque = self.maxTorques[name]
|
|
p.setJointMotorControl2(
|
|
self.robot, self.joints[name], p.POSITION_CONTROL, joints[name], force=maxTorque)
|
|
else:
|
|
p.setJointMotorControl2(
|
|
self.robot, self.joints[name], p.POSITION_CONTROL, joints[name])
|
|
|
|
applied[name] = p.getJointState(self.robot, self.joints[name])
|
|
else:
|
|
raise Exception("Can't find joint %s" % name)
|
|
|
|
return applied
|
|
|
|
def getJoints(self):
|
|
"""Get all the joints names
|
|
|
|
Returns:
|
|
list -- list of str, with joint names
|
|
"""
|
|
return self.joints.keys()
|
|
|
|
def getJointsInfos(self, name):
|
|
"""Get informations about a joint
|
|
|
|
Return:
|
|
list -- a list with key type, lowerLimit & upperLimit (if defined)
|
|
"""
|
|
|
|
return self.jointsInfos[name]
|
|
|
|
def getRobotMass(self):
|
|
"""Returns the robot mass
|
|
|
|
Returns:
|
|
float -- the robot mass (kg)
|
|
"""
|
|
if self.mass is None:
|
|
k = -1
|
|
self.mass = 0
|
|
while True:
|
|
if k == -1 or p.getLinkState(self.robot, k) is not None:
|
|
d = p.getDynamicsInfo(self.robot, k)
|
|
self.mass += d[0]
|
|
else:
|
|
break
|
|
k += 1
|
|
|
|
return self.mass
|
|
|
|
def getCenterOfMassPosition(self):
|
|
"""Returns center of mass of the robot
|
|
|
|
Returns:
|
|
pos -- (x, y, z) robot center of mass
|
|
"""
|
|
|
|
k = -1
|
|
mass = 0
|
|
com = np.array([0., 0., 0.])
|
|
while True:
|
|
if k == -1:
|
|
pos, _ = p.getBasePositionAndOrientation(self.robot)
|
|
else:
|
|
res = p.getLinkState(self.robot, k)
|
|
if res is None:
|
|
break
|
|
pos = res[0]
|
|
|
|
d = p.getDynamicsInfo(self.robot, k)
|
|
m = d[0]
|
|
com += np.array(pos) * m
|
|
mass += m
|
|
|
|
k += 1
|
|
|
|
return com / mass
|
|
|
|
def addDebugPosition(self, position, color=None, duration=30):
|
|
"""Adds a debug position to be drawn as a line
|
|
|
|
Arguments:
|
|
position {tuple} -- (x,y,z) (m)
|
|
|
|
Keyword Arguments:
|
|
color {tuple} -- (r,g,b) (0->1) (default: {None})
|
|
duration {float} -- line duration on screen before disapearing (default: {30})
|
|
"""
|
|
if color is None:
|
|
color = self.lineColors[self.currentLine % len(self.lineColors)]
|
|
|
|
if self.currentLine >= len(self.lines):
|
|
self.lines.append({})
|
|
|
|
self.lines[self.currentLine]['update'] = True
|
|
self.lines[self.currentLine]['to'] = position
|
|
self.lines[self.currentLine]['color'] = color
|
|
self.lines[self.currentLine]['duration'] = duration
|
|
|
|
self.currentLine += 1
|
|
|
|
def drawDebugLines(self):
|
|
"""Updates the drawing of debug lines"""
|
|
self.currentLine = 0
|
|
if time.time() - self.lastLinesDraw > 0.05:
|
|
for line in self.lines:
|
|
if 'from' in line:
|
|
if line['update'] == True:
|
|
p.addUserDebugLine(
|
|
line['from'], line['to'], line['color'], 2, line['duration'])
|
|
line['update'] = False
|
|
else:
|
|
del line['from']
|
|
line['from'] = line['to']
|
|
|
|
self.lastLinesDraw = time.time()
|
|
|
|
def contactPoints(self):
|
|
"""Gets all contact points and forces
|
|
|
|
Returns:
|
|
list -- list of entries (link_name, position in m, normal force vector, force in N)
|
|
"""
|
|
result = []
|
|
contacts = p.getContactPoints(bodyA=self.floor, bodyB=self.robot)
|
|
for contact in contacts:
|
|
link_index = contact[4]
|
|
if link_index >= 0:
|
|
link_name = (p.getJointInfo(
|
|
self.robot, link_index)[12]).decode()
|
|
else:
|
|
link_name = 'base'
|
|
result.append((link_name, contact[6], contact[7], contact[9]))
|
|
|
|
return result
|
|
|
|
def autoCollisions(self):
|
|
"""Returns the total amount of N in autocollisions (not with ground)
|
|
|
|
Returns:
|
|
float -- Newtons of collisions not with ground
|
|
"""
|
|
total = 0
|
|
for k in range(1, p.getNumJoints(self.robot)):
|
|
contacts = p.getContactPoints(bodyA=k)
|
|
for contact in contacts:
|
|
if contact[2] != self.floor:
|
|
total += contact[9]
|
|
return total
|
|
|
|
def addConstraint(self, frameA, frameB, constraint = p.JOINT_POINT2POINT):
|
|
"""Adds a constraint between two given frames
|
|
|
|
Args:
|
|
frameA (str): frame A name
|
|
frameB (str): frame A name
|
|
constraint (int, optional): pyBullet joint type. Defaults to p.JOINT_POINT2POINT.
|
|
|
|
Returns:
|
|
int: returns from pybullet createConstraint
|
|
"""
|
|
infosA = p.getJointInfo(self.robot, self.frames[frameA])
|
|
infosB = p.getJointInfo(self.robot, self.frames[frameB])
|
|
|
|
st = p.getLinkState(self.robot, infosA[16])
|
|
T_world_parentA = self.poseToMatrix(st[:2])
|
|
T_world_childA = self.poseToMatrix(self.getFrame(frameA))
|
|
T_parentA_childA = np.linalg.inv(T_world_parentA) * T_world_childA
|
|
childApose = self.matrixToPose(T_parentA_childA)
|
|
|
|
st = p.getLinkState(self.robot, infosB[16])
|
|
T_world_parentB = self.poseToMatrix(st[:2])
|
|
T_world_childB = self.poseToMatrix(self.getFrame(frameB))
|
|
T_parentB_childB = np.linalg.inv(T_world_parentB) * T_world_childB
|
|
childBpose = self.matrixToPose(T_parentB_childB)
|
|
|
|
c = p.createConstraint(
|
|
self.robot,
|
|
infosA[16],
|
|
self.robot,
|
|
infosB[16],
|
|
constraint,
|
|
[0.0, 0.0, 0.0],
|
|
childApose[0],
|
|
childBpose[0],
|
|
childApose[1],
|
|
childBpose[1],
|
|
)
|
|
|
|
p.changeConstraint(c, maxForce=1e3)
|
|
|
|
return c
|
|
|
|
def execute(self):
|
|
"""Executes the simulaiton infinitely (blocks)"""
|
|
while True:
|
|
self.tick()
|
|
|
|
def tick(self):
|
|
"""Ticks one step of simulation. If realTime is True, sleeps to compensate real time"""
|
|
self.t += self.dt
|
|
self.drawDebugLines()
|
|
|
|
p.stepSimulation()
|
|
delay = self.t - (time.time() - self.start)
|
|
if delay > 0 and self.realTime:
|
|
time.sleep(delay)
|