def initDisplay(self, loadModel):
RobotWrapper.initDisplay(self, loadModel=loadModel)
if loadModel and not hasattr(self, 'display'):
RobotWrapper.initDisplay(self, loadModel=False)
try:
# self.viewer.gui.deleteNode('world/mobilebasis', True)
self.viewer.gui.addBox('world/mobilebasis', .25, .25, .25,
[.8, .2, .2, 1])
self.viewer.gui.addCylinder('world/mobilewheel1', .05, .45,
[.1, .0, .0, 1])
self.viewer.gui.addCylinder('world/mobilewheel2', .05, .45,
[.1, .0, .0, 1])
self.viewer.gui.setStaticTransform('world/mobilebasis',
[.0, .0, .35, 1.0, .0, .0, .0])
self.viewer.gui.setStaticTransform(
'world/mobilewheel1', [+0.15, .0, .05, .7, .7, .0, .0])
self.viewer.gui.setStaticTransform(
'world/mobilewheel2', [-0.15, .0, .05, .7, .7, .0, .0])
self.viewer.gui.addXYZaxis('world/framebasis', [1., 0., 0., 1.],
.03, .1)
self.viewer.gui.addXYZaxis('world/frametool', [1., 0., 0., 1.],
.03, .1)
except:
print("Error when adding 3d objects in the viewer ... ignore")
def loadRobot(M0, name):
'''
This function load a UR5 robot n a new model, move the basis to placement <M0>
and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
It returns the robot wrapper (model,data).
'''
robot = RobotWrapper(urdf, [PKG])
robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
robot.initDisplay(loadModel=True, viewerRootNodeName="world/" + name)
return robot
def loadHRP(self, urdf, pkgs, loadModel):
'''Internal: load HRP-2 model from URDF.'''
robot = RobotWrapper(urdf, pkgs, root_joint=se3.JointModelFreeFlyer())
robot.initDisplay(loadModel=loadModel)
if 'viewer' not in robot.__dict__: robot.initDisplay()
for n in robot.visual_model.geometryObjects:
robot.viewer.gui.setVisibility(robot.viewerNodeNames(n), 'OFF')
robot.q0 = np.matrix([
0.,
0.,
0.648702,
0.,
0.,
0.,
1., # Free flyer
0.,
0.,
0.,
0., # Chest and head
0.261799,
0.17453,
0.,
-0.523599,
0.,
0.,
0.1, # Left arm
0.261799,
-0.17453,
0.,
-0.523599,
0.,
0.,
0.1, # Right arm
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0., # Left leg
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0., # Righ leg
]).T
self.hrpfull = robot
def initDisplay(self, loadModel):
RobotWrapper.initDisplay(self, loadModel=loadModel)
if loadModel and not hasattr(self, 'display'):
RobotWrapper.initDisplay(self, loadModel=False)
try:
# self.viewer.gui.deleteNode('world/mobilebasis', True)
self.viewer.gui.addBox('world/mobilebasis', .25, .25, .25, [.8, .2, .2, 1])
self.viewer.gui.addCylinder('world/mobilewheel1', .05, .45, [.1, .0, .0, 1])
self.viewer.gui.addCylinder('world/mobilewheel2', .05, .45, [.1, .0, .0, 1])
self.viewer.gui.setStaticTransform('world/mobilebasis', [.0, .0, .35, 1.0, .0, .0, .0])
self.viewer.gui.setStaticTransform('world/mobilewheel1', [+0.15, .0, .05, .7, .7, .0, .0])
self.viewer.gui.setStaticTransform('world/mobilewheel2', [-0.15, .0, .05, .7, .7, .0, .0])
self.viewer.gui.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1)
self.viewer.gui.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1)
except:
print("Error when adding 3d objects in the viewer ... ignore")
-0.418879,
0., # Left Leg
0.,
0.,
-0.453786,
0.872665,
-0.418879,
0. # Right Leg
)
#----------------PINOCCHIO----------------------------#
import pinocchio as se3
from pinocchio.robot_wrapper import RobotWrapper
pinocchioRobot = RobotWrapper(_urdfPath, _urdfDir, se3.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=True)
#----------------ROBOT - DEVICE AND DYNAMICS----------#
from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(_robotName, pinocchioRobot.model, pinocchioRobot.data,
_initialConfig, _OperationalPointsMap)
#-----------------------------------------------------#
#----------------SOT (SOLVER)-------------------------#
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
solver = initialize(robot)
#-----------------------------------------------------#
#----------------PG--------------------------------#
#from dynamic_graph.sot.pattern_generator.walking import initPg, initZMPRef, initWaistCoMTasks, initFeetTask, initPostureTask, pushTasks
from dynamic_graph.sot.pattern_generator.walking import walkNaveau, CreateEverythingForPG, walkFewSteps, walkAndrei
class PlayFromFile:
def __init__(self,log="/tmp/log_pickup.txt"):
self.wrapper = RobotWrapper("/local/mgeisert/models/ur5_quad.urdf")
self.client=Client()
self.client.gui.createWindow("w")
self.client.gui.createScene("world")
self.client.gui.addSceneToWindow("world",0L)
self.client.gui.addURDF("world/ur5","/local/mgeisert/models/ur5_quad.urdf","/local/mgeisert/models/universal_robot/")
self.client.gui.addSphere("world/sphere",0.07,[0.7,0.,0.,1.])
self.client.gui.createGroup("world/gripper")
self.client.gui.addLandmark("world/gripper",0.1)
self.wrapper.initDisplay("world/ur5")
self.list = []
file = open(log, "r")
configs = file.readlines()
list1 = []
tmp3 = 0.
for i in range(len(configs)):
tmp = np.fromstring(configs[i][2:], dtype=float, sep="\t")
tmp2 = []
if tmp[0]-tmp3>0.:
tmp2.extend([tmp[0]-tmp3])
tmp2.extend(tmp[1:4])
x = Quaternion().fromRPY(tmp[4],tmp[5],tmp[6])
tmp2.extend(x.array[1:4])
tmp2.extend([x.array[0]])
tmp2.extend(tmp[7:13])
tmp2.append(tmp[0])
tmp3 = tmp[0]
list1.append(tmp2)
self.list.append(list1)
def play(self, i=0, speed=1):
for config in self.list[i]:
q = np.matrix([[config[1]],[config[2]],[config[3]],[config[4]],[config[5]],[config[6]],[config[7]],[config[8]],[config[9]],[config[10]],[config[11]],[config[12]],[config[13]]])
self.wrapper.display(q)
x = np.matrix([[0],[0.15],[0]])
x = self.wrapper.data.oMi[7]*x
x = x.transpose()
x = x.tolist()[0]
print x
quat = Quaternion(self.wrapper.data.oMi[7].rotation)
x.extend(quat.array[0:4])
self.client.gui.applyConfiguration("world/gripper",x)
if config[14]<3:
self.client.gui.applyConfiguration("world/sphere",[2,-1,-1,1,0,0,0])
elif config[14]<7:
self.client.gui.applyConfiguration("world/sphere",x)
self.client.gui.refresh()
time.sleep(config[0]*speed)
def display(self,k, i=0):
config = self.list[i][k]
q = np.matrix([[config[1]],[config[2]],[config[3]],[config[4]],[config[5]],[config[6]],[config[7]],[config[8]],[config[9]],[config[10]],[config[11]],[config[12]],[config[13]]])
self.wrapper.display(q)
def reload(self,file="/tmp/log_pickup.txt"):
file = open(file, "r")
configs = file.readlines()
list1 = []
tmp3 = 0.
for i in range(len(configs)):
tmp = np.fromstring(configs[i][2:], dtype=float, sep="\t")
tmp2 = []
if tmp[0]-tmp3>0.:
tmp2.extend([tmp[0]-tmp3])
tmp2.extend(tmp[1:4])
x = Quaternion().fromRPY(tmp[4],tmp[5],tmp[6])
tmp2.extend(x.array[1:4])
tmp2.extend([x.array[0]])
tmp2.extend(tmp[7:13])
tmp2.append(tmp[0])
tmp3 = tmp[0]
list1.append(tmp2)
self.list.insert(0,list1)
model_path,
])
#model_path = os.getcwd()+'/../data'
#robot = RobotWrapper(model_path+'/pr2_description/urdf/pr2.urdf', [ model_path, ]);
robot.addAllCollisionPairs()
NON_ACTIVE_COLLISION_PAIRS = []
for i in range(len(robot.collision_data.activeCollisionPairs)):
if (i not in ACTIVE_COLLISION_PAIRS):
NON_ACTIVE_COLLISION_PAIRS += [i]
robot.deactivateCollisionPairs(NON_ACTIVE_COLLISION_PAIRS)
dcrba = DCRBA(robot)
coriolis = Coriolis(robot)
drnea = DRNEA(robot)
robot.initDisplay(loadModel=True)
#robot.loadDisplayModel("world/pinocchio", "pinocchio", model_path)
robot.viewer.gui.setLightingMode('world/floor', 'OFF')
#print robot.model
Q_MIN = robot.model.lowerPositionLimit
Q_MAX = robot.model.upperPositionLimit
DQ_MAX = robot.model.velocityLimit
TAU_MAX = robot.model.effortLimit
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
robot.display(q)
# Display the robot in Gepetto-Viewer.
nq = robot.nq
nv = robot.nv
v = mat_zeros(robot.nv)
dv = mat_zeros(robot.nv)
import pinocchio as se3
from pinocchio.robot_wrapper import RobotWrapper
from pinocchio.utils import *
from time import sleep
from numpy.linalg import inv
import math
pkg = 'models/'
urdf = pkg + 'ur_description/urdf/ur5_gripper.urdf'
robot = RobotWrapper( urdf, [ pkg, ] )
robot.initDisplay( loadModel = True )
if 'viewer' not in robot.__dict__: robot.initDisplay()
gv = robot.viewer.gui
NQ = robot.model.nq
NV = robot.model.nv
###
### PICK #############################################################
###
# Add a red box
boxID = "world/box"
rgbt = [ 1.0, 0.2, 0.2, 1.0 ] # red-green-blue-transparency
try:
gv.addBox( boxID, 0.05, 0.1, 0.1, rgbt ) # id, dim1, dim2, dim3, color
except:
print "Box already exists in viewer ... skip"
# Place the box at the position ( 0.5, 0.1, 0.2 )
)
#-----------------------------------------------------------------------------
#---- ROBOT SPECIFICATIONS----------------------------------------------------
#-----------------------------------------------------------------------------
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as pin
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
pinocchioRobot = RobotWrapper(urdfPath, urdfDir, pin.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(fromSotToPinocchio(initialConfig))
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(robotName, pinocchioRobot.model,
pinocchioRobot.data, initialConfig, OperationalPointsMap)
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
import unittest
import pinocchio as se3
import numpy as np
import os
from pinocchio.robot_wrapper import RobotWrapper
# Warning : the paths are here hard-coded. This file is only here as an example
romeo_model_path = os.path.abspath(
os.path.join(current_file, '../models/romeo/romeo_description'))
romeo_model_file = romeo_model_path + "/urdf/romeo.urdf"
list_hints = [romeo_model_path, "titi"]
robot = RobotWrapper(romeo_model_file, list_hints, se3.JointModelFreeFlyer())
robot.initDisplay()
robot.loadDisplayModel("world/pinocchio")
q0 = np.matrix([
0,
0,
0.840252,
0,
0,
0,
1, # Free flyer
0,
0,
-0.3490658,
0.6981317,
-0.3490658,
0.,
0. #Head
)
#-----------------------------------------------------------------------------
#---- ROBOT SPECIFICATIONS----------------------------------------------------
#-----------------------------------------------------------------------------
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as se3
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
pinocchioRobot = RobotWrapper(URDFPATH, URDFDIR, se3.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=DISPLAY)
if DISPLAY:
pinocchioRobot.display(fromSotToPinocchio(halfSitting))
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data,
halfSitting, OperationalPointsMap)
# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)
