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
class TestFrameBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m6zero = zero([6, 6])
m3ones = eye(3)
m4ones = eye(4)
current_file = os.path.dirname(os.path.abspath(__file__))
pinocchio_models_dir = os.path.abspath(
os.path.join(current_file, '../models/romeo'))
romeo_model_path = os.path.abspath(
os.path.join(pinocchio_mdoels_dir, '/urdf/romeo.urdf'))
hint_list = [pinocchio_models_dir, "wrong/hint"] # hint list
robot = RobotWrapper(romeo_model_path, hint_list,
se3.JointModelFreeFlyer())
def test_type_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.type == se3.FrameType.JOINT)
f.type = se3.FrameType.BODY
self.assertTrue(f.type == se3.FrameType.BODY)
def test_name_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.name == 'LHipYaw')
f.name = 'new_hip_frame'
self.assertTrue(f.name == 'new_hip_frame')
def test_parent_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.parent == 2)
f.parent = 5
self.assertTrue(f.parent == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0, 0, 0], np.double))
new_m = se3.SE3(rand([3, 3]), rand(3))
f = self.robot.model.frames[2]
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous,
new_m.homogeneous))
class TestGeometryObjectBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m6zero = zero([6, 6])
m3ones = eye(3)
m4ones = eye(4)
current_file = os.path.dirname(os.path.abspath(__file__))
pinocchio_models_dir = os.path.abspath(
os.path.join(current_file, '../models/romeo'))
romeo_model_path = os.path.abspath(pinocchio_models_dir,
'/urdf/romeo.urdf')
hint_list = [pinocchio_models_dir, "wrong/hint"] # hint list
robot = RobotWrapper(romeo_model_path, hint_list,
se3.JointModelFreeFlyer())
def test_name_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.name == 'LHipPitchLink_0')
col.name = 'new_collision_name'
self.assertTrue(col.name == 'new_collision_name')
def test_parent_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.parentJoint == 4)
col.parentJoint = 5
self.assertTrue(col.parentJoint == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3, 3]), rand(3))
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(np.allclose(col.placement.homogeneous, m.homogeneous))
col.placement = new_m
self.assertTrue(
np.allclose(col.placement.homogeneous, new_m.homogeneous))
def test_meshpath_get(self):
expected_mesh_path = os.path.join(
self.pinocchio_models_dir, 'meshes/romeo/collision/LHipPitch.dae')
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.meshPath == expected_mesh_path)
class TestFrameBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m6zero = zero([6, 6])
m3ones = eye(3)
m4ones = eye(4)
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'romeo_description/urdf/romeo.urdf')
robot = RobotWrapper(URDF, [PKG])
def test_type_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.type == se3.FrameType.JOINT)
f.type = se3.FrameType.BODY
self.assertTrue(f.type == se3.FrameType.BODY)
def test_name_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.name == 'LHipYaw')
f.name = 'new_hip_frame'
self.assertTrue(f.name == 'new_hip_frame')
def test_parent_get_set(self):
f = self.robot.model.frames[5]
self.assertTrue(f.parent == 2)
f.parent = 5
self.assertTrue(f.parent == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0, 0, 0], np.double))
new_m = se3.SE3(rand([3, 3]), rand(3))
f = self.robot.model.frames[2]
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous,
new_m.homogeneous))
class TestFrameBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3, 3])
m6zero = zero([6, 6])
m3ones = eye(3)
m4ones = eye(4)
current_file = os.path.dirname(os.path.abspath(__file__))
pinocchio_models_dir = os.path.abspath(
os.path.join(current_file, '../models'))
romeo_model_path = os.path.abspath(
os.path.join(current_file, '../models/romeo.urdf'))
hint_list = [pinocchio_models_dir, "wrong/hint"] # hint list
robot = RobotWrapper(romeo_model_path, hint_list,
se3.JointModelFreeFlyer())
def test_name_get_set(self):
f = self.robot.model.operational_frames[1]
self.assertTrue(f.name == 'r_sole_joint')
f.name = 'new_sole_joint'
self.assertTrue(f.name == 'new_sole_joint')
def test_parent_get_set(self):
f = self.robot.model.operational_frames[1]
self.assertTrue(f.parent == 13)
f.parent = 5
self.assertTrue(f.parent == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, np.array([0, 0, -0.0684], np.double))
new_m = se3.SE3(rand([3, 3]), rand(3))
f = self.robot.model.operational_frames[1]
self.assertTrue(np.allclose(f.placement.homogeneous, m.homogeneous))
f.placement = new_m
self.assertTrue(np.allclose(f.placement.homogeneous,
new_m.homogeneous))
class TestGeometryObjectBindings(unittest.TestCase):
v3zero = zero(3)
v6zero = zero(6)
v3ones = ones(3)
m3zero = zero([3,3])
m6zero = zero([6,6])
m3ones = eye(3)
m4ones = eye(4)
PKG = '/opt/openrobots/share'
URDF = join(PKG, 'romeo_description/urdf/romeo.urdf')
robot = RobotWrapper(URDF, [PKG])
def test_name_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.name == 'LHipPitchLink_0')
col.name = 'new_collision_name'
self.assertTrue(col.name == 'new_collision_name')
def test_parent_get_set(self):
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.parentJoint == 4)
col.parentJoint = 5
self.assertTrue(col.parentJoint == 5)
def test_placement_get_set(self):
m = se3.SE3(self.m3ones, self.v3zero)
new_m = se3.SE3(rand([3,3]), rand(3))
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous))
col.placement = new_m
self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous))
def test_meshpath_get(self):
expected_mesh_path = os.path.join(self.pinocchio_models_dir,'meshes/romeo/collision/LHipPitch.dae')
col = self.robot.collision_model.geometryObjects[1]
self.assertTrue(col.meshPath == expected_mesh_path)
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 )
def __init__(self,
jiminy_model,
use_theoretical_model=False,
urdf_rgba=None,
robot_index=0,
backend=None,
window_name='python-pinocchio',
scene_name='world'):
# Backup some user arguments
self.urdf_path = jiminy_model.urdf_path
self.scene_name = scene_name
self.window_name = window_name
self.use_theoretical_model = use_theoretical_model
# Extract the right Pinocchio model
if self.use_theoretical_model:
self.pinocchio_model = jiminy_model.pinocchio_model_th
self.pinocchio_data = jiminy_model.pinocchio_data_th
else:
self.pinocchio_model = jiminy_model.pinocchio_model
self.pinocchio_data = jiminy_model.pinocchio_data
# Select the desired backend
if backend is None:
if Viewer.backend is None:
backend = 'meshcat' if Viewer._is_notebook() else 'gepetto-gui'
else:
backend = Viewer.backend
# Update the backend currently running, if any
if (Viewer.backend != backend) and \
(Viewer._backend_obj is not None or \
Viewer._backend_proc is not None):
Viewer.close()
print("Different backend already running. Closing it...")
Viewer.backend = backend
# Check if the backend is still available, if any
if Viewer._backend_obj is not None and Viewer._backend_proc is not None:
if Viewer._backend_proc.poll() is not None:
Viewer._backend_obj = None
Viewer._backend_proc = None
# Access the current backend or create one if none is available
try:
if (Viewer.backend == 'gepetto-gui'):
if Viewer._backend_obj is None:
Viewer._backend_obj, Viewer._backend_proc = \
Viewer._get_gepetto_client(True)
if Viewer._backend_obj is not None:
self._client = Viewer._backend_obj.gui
else:
raise RuntimeError("Impossible to open Gepetto-viewer.")
else:
from pinocchio.visualize import MeshcatVisualizer
from pinocchio.shortcuts import createDatas
if Viewer._backend_obj is None:
Viewer._create_meshcat_backend()
if Viewer._is_notebook():
Viewer.display_jupyter_cell()
else:
Viewer._backend_obj.open()
self._client = MeshcatVisualizer(self.pinocchio_model, None,
None)
self._client.viewer = Viewer._backend_obj
except:
raise RuntimeError("Impossible to load backend.")
# Create a RobotWrapper
robot_name = "robot_" + str(robot_index)
if (Viewer.backend == 'gepetto-gui'):
Viewer._delete_gepetto_nodes_viewer(scene_name + '/' + robot_name)
if (urdf_rgba is not None):
alpha = urdf_rgba[3]
self.urdf_path = Viewer._get_colorized_urdf(
self.urdf_path, urdf_rgba[:3])
else:
alpha = 1.0
collision_model = pin.buildGeomFromUrdf(
self.pinocchio_model, self.urdf_path,
os.environ.get('JIMINY_MESH_PATH', []), pin.GeometryType.COLLISION)
visual_model = pin.buildGeomFromUrdf(
self.pinocchio_model, self.urdf_path,
os.environ.get('JIMINY_MESH_PATH', []), pin.GeometryType.VISUAL)
self._rb = RobotWrapper(model=self.pinocchio_model,
collision_model=collision_model,
visual_model=visual_model)
if not self.use_theoretical_model:
self._rb.data = jiminy_model.pinocchio_data
self.pinocchio_data = self._rb.data
# Load robot in the backend viewer
if (Viewer.backend == 'gepetto-gui'):
if not scene_name in self._client.getSceneList():
self._client.createSceneWithFloor(scene_name)
if not window_name in self._client.getWindowList():
self._window_id = self._client.createWindow(window_name)
self._client.addSceneToWindow(scene_name, self._window_id)
self._client.createGroup(scene_name + '/' + scene_name)
self._client.addLandmark(scene_name + '/' + scene_name, 0.1)
else:
self._window_id = int(
np.where([
name == window_name
for name in self._client.getWindowList()
])[0][0])
self._rb.initViewer(windowName=window_name,
sceneName=scene_name,
loadModel=False)
self._rb.loadViewerModel(robot_name)
self._client.setFloatProperty(scene_name + '/' + robot_name,
'Transparency', 1 - alpha)
else:
self._client.collision_model = collision_model
self._client.visual_model = visual_model
self._client.data, self._client.collision_data, self._client.visual_data = \
createDatas(self.pinocchio_model, collision_model, visual_model)
self._client.loadViewerModel(rootNodeName=robot_name,
color=urdf_rgba)
self._rb.viz = self._client
self.Ycrb = Ycrb
self.Yvx = Yvx
return R
# -----------------------------------------------------------------------------------
# -----------------------------------------------------------------------------------
# -----------------------------------------------------------------------------------
# --- UNIT TEST ---
if __name__ == '__main__':
np.random.seed(0)
robot = RobotWrapper(
'/home/nmansard/src/pinocchio/pinocchio/models/romeo/urdf/romeo.urdf',
[
'/home/nmansard/src/pinocchio/pinocchio/models/romeo/',
], se3.JointModelFreeFlyer())
q = rand(robot.model.nq)
q[3:7] /= norm(q[3:7])
vq = rand(robot.model.nv)
# --- HESSIAN ---
# --- HESSIAN ---
# --- HESSIAN ---
H = hessian(robot, q)
# Compute the Hessian matrix H using RNEA, so that acor = v*H*v
vq1 = vq * 0
Htrue = np.zeros([6, robot.model.nv, robot.model.nv])
for joint_i in range(1, robot.model.njoints):
'mass': 1.0,
},
]
operational_frames = [
{
'name': "center",
'parent': "base",
'placement': {
'z': 0.0
},
},
]
ball = {'joints': joints, 'operational_frames': operational_frames}
print("test set up")
robot = RobotWrapper(ball)
#robot = RobotWrapper(ball, name='ball', display=None)
print(robot.data.com[0])
robot.data = se3.Data(robot.model)
print(robot.data.com[0])
if not robot.model.check(robot.data):
print("Python data not consistent with model")
print("Create simple simulator")
sim = consim.build_simple_simulator(1e-3, 8, robot.model, robot.data, 0., 0.,
0., 0., 0., 0.)
def __init__(self,
robot,
use_theoretical_model=False,
mesh_root_path=None,
urdf_rgba=None,
lock=None,
backend=None,
robot_name="robot",
window_name='jiminy',
scene_name='world'):
"""
@brief Constructor.
@param robot The jiminy.Robot to display.
@param use_theoretical_model Whether to use the theoretical (rigid) model or the flexible model for this robot.
@param mesh_root_path Optional, path to the folder containing the URDF meshes.
@param urdf_rgba Color to use to display this robot (rgba).
- @param lock Custom threading.Lock
- Optional: Only required for parallel rendering using multiprocessing.
It is required since some backends does not support multiple
simultaneous connections (e.g. corbasever).
@param backend The name of the desired backend to use for rendering.
Optional, either 'gepetto-gui' or 'meshcat' ('panda3d' available soon).
@param robot_name Unique robot name, to identify each robot in the viewer.
@param scene_name Scene name, used only when gepetto-gui is used as backend.
@param window_name Window name, used only when gepetto-gui is used as backend.
Note that it is not allowed to be equal to the window name.
"""
# Backup some user arguments
self.urdf_path = robot.urdf_path
self.robot_name = robot_name
self.scene_name = scene_name
self.window_name = window_name
self.use_theoretical_model = use_theoretical_model
self._lock = lock if lock is not None else Viewer._lock
if self.scene_name == self.window_name:
raise ValueError(
"Please, choose a different name for the scene and the window."
)
# Extract the right Pinocchio model
if self.use_theoretical_model:
self.pinocchio_model = robot.pinocchio_model_th
self.pinocchio_data = robot.pinocchio_data_th
else:
self.pinocchio_model = robot.pinocchio_model
self.pinocchio_data = robot.pinocchio_data
# Select the desired backend
if backend is None:
if Viewer.backend is None:
if Viewer._is_notebook() or (not 'gepetto-gui'
in backends_available):
backend = 'meshcat'
else:
backend = 'gepetto-gui'
else:
backend = Viewer.backend
else:
if not backend in backends_available:
raise ValueError("%s backend not available." % backend)
# Update the backend currently running, if any
if (Viewer.backend != backend) and \
(Viewer._backend_obj is not None or \
Viewer._backend_proc is not None):
Viewer.close()
print("Different backend already running. Closing it...")
Viewer.backend = backend
# Check if the backend is still available, if any
if Viewer._backend_obj is not None and Viewer._backend_proc is not None:
is_backend_running = True
if Viewer._backend_proc.poll() is not None:
is_backend_running = False
if (Viewer.backend == 'gepetto-gui'):
from omniORB.CORBA import TRANSIENT as gepetto_server_error
try:
Viewer._backend_obj.gui.refresh()
except gepetto_server_error:
is_backend_running = False
if not is_backend_running:
Viewer._backend_obj = None
Viewer._backend_proc = None
Viewer._backend_exception = None
else:
is_backend_running = False
# Access the current backend or create one if none is available
try:
if (Viewer.backend == 'gepetto-gui'):
import omniORB
Viewer._backend_exception = omniORB.CORBA.COMM_FAILURE
if Viewer._backend_obj is None:
Viewer._backend_obj, Viewer._backend_proc = \
Viewer._get_gepetto_client(True)
is_backend_running = Viewer._backend_proc is None
if Viewer._backend_obj is not None:
self._client = Viewer._backend_obj.gui
else:
raise RuntimeError("Impossible to open Gepetto-viewer.")
if not scene_name in self._client.getSceneList():
self._client.createSceneWithFloor(scene_name)
if not window_name in self._client.getWindowList():
self._window_id = self._client.createWindow(window_name)
self._client.addSceneToWindow(scene_name, self._window_id)
self._client.createGroup(scene_name + '/' + scene_name)
self._client.addLandmark(scene_name + '/' + scene_name,
0.1)
else:
self._window_id = int(
np.where([
name == window_name
for name in self._client.getWindowList()
])[0][0])
# Set the default camera pose if the viewer is not running before
if not is_backend_running:
self.setCameraTransform(
translation=DEFAULT_CAMERA_XYZRPY_OFFSET_GEPETTO[:3],
rotation=DEFAULT_CAMERA_XYZRPY_OFFSET_GEPETTO[3:])
else:
from pinocchio.visualize import MeshcatVisualizer
from pinocchio.shortcuts import createDatas
if Viewer._backend_obj is None:
Viewer._create_meshcat_backend()
if Viewer._is_notebook():
Viewer.display_jupyter_cell()
else:
Viewer._backend_obj.open()
self._client = MeshcatVisualizer(self.pinocchio_model, None,
None)
self._client.viewer = Viewer._backend_obj
except:
raise RuntimeError("Impossible to load backend.")
# Create a RobotWrapper
root_path = mesh_root_path if mesh_root_path is not None else os.environ.get(
'JIMINY_MESH_PATH', [])
if (Viewer.backend == 'gepetto-gui'):
Viewer._delete_gepetto_nodes_viewer(scene_name + '/' +
self.robot_name)
if (urdf_rgba is not None):
alpha = urdf_rgba[3]
self.urdf_path = Viewer._get_colorized_urdf(
self.urdf_path, urdf_rgba[:3], root_path)
else:
alpha = 1.0
collision_model = pin.buildGeomFromUrdf(self.pinocchio_model,
self.urdf_path, root_path,
pin.GeometryType.COLLISION)
visual_model = pin.buildGeomFromUrdf(self.pinocchio_model,
self.urdf_path, root_path,
pin.GeometryType.VISUAL)
self._rb = RobotWrapper(model=self.pinocchio_model,
collision_model=collision_model,
visual_model=visual_model)
if not self.use_theoretical_model:
self._rb.data = robot.pinocchio_data
self.pinocchio_data = self._rb.data
# Load robot in the backend viewer
if (Viewer.backend == 'gepetto-gui'):
self._rb.initViewer(windowName=window_name,
sceneName=scene_name,
loadModel=False)
self._rb.loadViewerModel(self.robot_name)
self._client.setFloatProperty(scene_name + '/' + self.robot_name,
'Transparency', 1 - alpha)
else:
self._client.collision_model = collision_model
self._client.visual_model = visual_model
self._client.data, self._client.collision_data, self._client.visual_data = \
createDatas(self.pinocchio_model, collision_model, visual_model)
self._client.loadViewerModel(rootNodeName=self.robot_name,
color=urdf_rgba)
self._rb.viz = self._client
import locomote2
from hyq_config import *
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as se3
from numpy.linalg import norm, pinv, inv
robot = RobotWrapper(urdf_model_path,
mesh_dir,
root_joint=se3.JointModelFreeFlyer())
model = robot.model
data = robot.data
com_init = robot.com(robot.q0)
com_final = com_init + np.matrix([0.01, 0.0, 0]).transpose()
MASS = robot.data.mass[0]
robot.initDisplay(loadModel=True)
robot.display(q0)
# for generating Time Optimization Problem
nstep = 5
tp = locomote2.TimeoptProblem(nstep)
tp.init_com = com_init
tp.final_com = com_final
tp.mass = MASS
print(com_init)
# Adding each contact sequence
phase = locomote2.TimeoptPhase()
phase.ee_id = locomote2.EndeffectorID.LF
0.872665,
-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
cross(rand(3), rand(3)) # Cross product of R^3
rotate('x', 0.4) # Build a rotation matrix of 0.4rad around X.
import pinocchio as se3
R = eye(3)
p = zero(3)
M0 = se3.SE3(R, p)
M = se3.SE3.Random()
M.translation = p
M.rotation = R
v = zero(3)
w = zero(3)
nu0 = se3.Motion(v, w)
nu = se3.Motion.Random()
nu.linear = v
nu.angular = w
f = zero(3)
tau = zero(3)
phi0 = se3.Force(f, tau)
phi = se3.Force.Random()
phi.linear = f
phi.angular = tau
from pinocchio.robot_wrapper import RobotWrapper
from os.path import join
PKG = '/home/alumno04/dev_samir/ros/sawyer_ws/src/'
URDF = join(PKG, '/sawyer_robot/sawyer_description/urdf/model1.urdf')
robot = RobotWrapper(URDF, [PKG])
def __init__(self, name, initialConfig, device=None, tracer=None):
self.OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint',
'right-wrist': 'arm_right_7_joint',
'left-ankle': 'leg_left_6_joint',
'right-ankle': 'leg_right_6_joint',
'gaze': 'head_2_joint',
'waist': 'root_joint',
'chest': 'torso_2_joint'
}
from rospkg import RosPack
rospack = RosPack()
urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data') + "/../"]
# Create a wrapper to access the dynamic model provided through an urdf file.
from pinocchio.robot_wrapper import RobotWrapper
import pinocchio as se3
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
pinocchioRobot = RobotWrapper()
pinocchioRobot.initFromURDF(urdfPath, urdfDir,
se3.JointModelFreeFlyer())
self.pinocchioModel = pinocchioRobot.model
self.pinocchioData = pinocchioRobot.data
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
# Create rigid body dynamics model and data (pinocchio)
self.dynamic = DynamicPinocchio(self.name + "_dynamic")
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
# Initialize device
self.device = device
self.timeStep = self.device.getTimeStep()
self.device.resize(self.dynamic.getDimension())
# TODO For position limit, we remove the first value to get
# a vector of the good size because SoT use euler angles and not
# quaternions...
self.device.setPositionBounds(
self.pinocchioModel.lowerPositionLimit.T.tolist()[0][1:],
self.pinocchioModel.upperPositionLimit.T.tolist()[0][1:])
self.device.setVelocityBounds(
(-self.pinocchioModel.velocityLimit).T.tolist()[0],
self.pinocchioModel.velocityLimit.T.tolist()[0])
self.device.setTorqueBounds(
(-self.pinocchioModel.effortLimit).T.tolist()[0],
self.pinocchioModel.effortLimit.T.tolist()[0])
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
self.AdditionalFrames.append(
("leftFootForceSensor", self.forceSensorInLeftAnkle,
self.OperationalPointsMap["left-ankle"]))
self.AdditionalFrames.append(
("rightFootForceSensor", self.forceSensorInRightAnkle,
self.OperationalPointsMap["right-ankle"]))
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
self.dynamic.displayModel()
# Initialize velocity derivator if chosen
if self.enableVelocityDerivator:
self.velocityDerivator = Derivator_of_Vector('velocityDerivator')
self.velocityDerivator.dt.value = self.timeStep
plug(self.device.state, self.velocityDerivator.sin)
plug(self.velocityDerivator.sout, self.dynamic.velocity)
else:
self.dynamic.velocity.value = self.dimension * (0., )
# Initialize acceleration derivator if chosen
if self.enableAccelerationDerivator:
self.accelerationDerivator = \
Derivator_of_Vector('accelerationDerivator')
self.accelerationDerivator.dt.value = self.timeStep
plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
else:
self.dynamic.acceleration.value = self.dimension * (0., )
# Create operational points based on operational points map (if provided)
if self.OperationalPointsMap is not None:
self.initializeOpPoints()
0.1, #Right Arm
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())
MOTOR_INERTIA = 4.59e-6
GEAR_RATIO = 66
DDQ_MAX = np.array([30.0, 60.0])
DDQ_MAX = np.array([10.0, 30.0, 20.0, 20.0, 30.0, 30.0, 30.0])
''' END OF USER PARAMETERS '''
print(
"Gonna sample %d random joint configuration to compute bounds on the Mass matrix and bias forces"
% (N_SAMPLING))
np.set_printoptions(precision=2, suppress=True)
# Sample the joint space of the robot
model_path = os.getcwd() + '/../data/baxter'
#robot = RobotWrapper(model_path+'/baxter_description/urdf/baxter_2_dof.urdf', [ model_path, ]);
robot = RobotWrapper(model_path + '/baxter_description/urdf/baxter_1_arm.urdf',
[
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)
WITH_UR5 = True
WITH_ROMEO = False
if WITH_UR5:
from pinocchio.robot_wrapper import RobotWrapper
path = 'models/'
urdf = path + '/ur_description/urdf/ur5_gripper.urdf'
robot = RobotWrapper(urdf, [
path,
])
if WITH_ROMEO:
from pinocchio.romeo_wrapper import RomeoWrapper
path = 'models/romeo/'
urdf = path + 'urdf/romeo.urdf'
# Explicitly specify that the first joint is a free flyer.
robot = RomeoWrapper(urdf, [
path,
]) # Load urdf model
robot.initDisplay(loadModel=True)
robot.display(robot.q0)
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., # Right gripper
0.,
0. # Head
)
#-----------------------------------------------------------------------------
#---- 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())
plug(sot.control, robot.device.control)
RorL = hyst(ForceRZ.A1 - ForceLZ.A1, -200, 200)
#plt.plot(ForceRZ)
#plt.plot(ForceLZ)
#plt.plot(RorL*500)
#plt.show()
urdf = '/opt/openrobots/share/hrp2_14_description/urdf/hrp2_14.urdf'
pkgs = [
'/opt/openrobots/share',
]
#(self, filename, package_dirs=package_dirs, root_joint=se3.JointModelFreeFlyer())
robot = RobotWrapper(urdf,
package_dirs=pkgs,
root_joint=se3.JointModelFreeFlyer())
#robot.initDisplay()#loadModel=True)
T = qs.shape[0]
RLEG = qs[:, :6]
LLEG = qs[:, 6:12]
CHEST = qs[:, 12:14]
HEAD = qs[:, 14:16]
RARM = qs[:, 16:23]
LARM = qs[:, 23:]
FF = qs[:, :7] * 0
FF[:, 6] = 1
qs = np.hstack([FF, CHEST, HEAD, LARM, RARM, LLEG, RLEG])
# NOTE: this example needs gepetto-gui to be installed
# usage: launch gepetto-gui and then run this test
import unittest
import pinocchio as se3
import numpy as np
import os
from pinocchio.robot_wrapper import RobotWrapper
current_file = os.path.dirname(os.path.abspath(__file__))
romeo_model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
romeo_model_path = os.path.abspath(os.path.join(romeo_model_dir, 'romeo_description/urdf/romeo.urdf'))
hint_list = [romeo_model_dir, "wrong/hint"] # hint list
robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
robot.initDisplay()
robot.loadDisplayModel("pinocchio")
q0 = np.matrix([
0, 0, 0.840252, 0, 0, 0, 1, # Free flyer
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # left leg
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # right leg
0, # chest
1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # left arm
0, 0, 0, 0, # head
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # right arm
]).T
robot.display(q0)
