def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', se3ToXYZQUAT(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', se3ToXYZQUAT(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', se3ToXYZQUAT(M1))
self.viewer.gui.refresh()
pinocchio.updateFramePlacements(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis', se3ToXYZQUAT(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool', se3ToXYZQUAT(self.data.oMf[-1]))
self.viewer.gui.refresh()
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', pin.se3ToXYZQUATtuple(M1))
self.viewer.gui.refresh()
pin.framesKinematics(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis', pin.se3ToXYZQUATtuple(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool', pin.se3ToXYZQUATtuple(self.data.oMf[-1]))
self.viewer.gui.refresh()
def display(self, q):
RobotWrapper.display(self, q)
M1 = self.data.oMi[1]
self.viewer.gui.applyConfiguration('world/mobilebasis', M2gv(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel1', M2gv(M1))
self.viewer.gui.applyConfiguration('world/mobilewheel2', M2gv(M1))
self.viewer.gui.refresh()
se3.framesKinematics(self.model, self.data)
self.viewer.gui.applyConfiguration('world/framebasis',
M2gv(self.data.oMf[-2]))
self.viewer.gui.applyConfiguration('world/frametool',
M2gv(self.data.oMf[-1]))
self.viewer.gui.refresh()
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)
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)
ddq_ub_min = mat_zeros(nq) + 1e10
ddq_lb_max = mat_zeros(nq) - 1e10
M_max = mat_zeros((nv, nv)) - 1e10
M_min = mat_zeros((nv, nv)) + 1e10
h_max = mat_zeros(nv) - 1e10
h_min = mat_zeros(nv) + 1e10
dh_dq_max = mat_zeros((nv, nq)) - 1e10
dh_dq_min = mat_zeros((nv, nq)) + 1e10
dh_dv_max = mat_zeros((nv, nv)) - 1e10
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)
robot.initDisplay(loadModel=True)
robot.viewer.gui.setLightingMode('world/floor', 'OFF')
#print robot.model
Q_MIN = robot.model.lowerPositionLimit
Q_MAX = robot.model.upperPositionLimit
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
robot.display(q)
# Display the robot in Gepetto-Viewer.
nq = robot.nq
rightAnswerCounter = 0
for i in range(N_SAMPLING):
if (rightAnswerCounter > 10):
ans = input(
"The model has been right for %d times in a row. Do you wanna stop?"
% rightAnswerCounter)
if 'y' in ans:
print("Stopping script")
print("Active collision pairs are: [", end=' ')
for (cp, active) in enumerate(
robot.collision_data.activeCollisionPairs):
if (active):
class Viewer:
backend = None
port_forwarding = None
_backend_obj = None
_backend_proc = None
## Unique threading.Lock for every simulation.
# It is required for parallel rendering since corbaserver does not support multiple connection simultaneously.
_lock = Lock()
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
@staticmethod
def _create_meshcat_backend():
import meshcat
from contextlib import redirect_stdout
with redirect_stdout(None):
Viewer._backend_obj = meshcat.Visualizer()
Viewer._backend_proc = Viewer._backend_obj.window.server_proc
@staticmethod
def reset_port_forwarding(port_forwarding=None):
Viewer.port_forwarding = port_forwarding
@staticmethod
def display_jupyter_cell(height=600,
width=900,
force_create_backend=False):
if Viewer.backend == 'meshcat' and Viewer._is_notebook():
from IPython.core.display import HTML, display as ipython_display
if Viewer._backend_obj is None:
if force_create_backend:
Viewer._create_meshcat_backend()
else:
raise ValueError(
"No meshcat backend available and 'force_create_backend' is set to False."
)
viewer_url = Viewer._backend_obj.url()
if Viewer.port_forwarding is not None:
url_port_pattern = '(?<=:)[0-9]+(?=/)'
port_localhost = int(
re.search(url_port_pattern, viewer_url).group())
if port_localhost in Viewer.port_forwarding.keys():
viewer_url = re.sub(
url_port_pattern,
str(Viewer.port_forwarding[port_localhost]),
viewer_url)
else:
print(
"Port forwarding defined but no port mapping associated with {port_localhost}."
)
jupyter_html = f'\n<div style="height: {height}px; width: {width}px; overflow-x: auto; overflow-y: hidden; resize: both">\
\n<iframe src="{viewer_url}" style="width: 100%; height: 100%; border: none">\
</iframe>\n</div>\n'
ipython_display(HTML(jupyter_html))
else:
raise ValueError(
"Display in a Jupyter cell is only available using 'meshcat' backend and within a Jupyter notebook."
)
@staticmethod
def close():
if Viewer._backend_proc is not None:
Viewer._backend_proc.terminate()
Viewer._backend_proc = None
Viewer._backend_obj = None
@staticmethod
def _is_notebook():
try:
shell = get_ipython().__class__.__name__
if shell == 'ZMQInteractiveShell':
return True # Jupyter notebook or qtconsole
elif shell == 'TerminalInteractiveShell':
return False # Terminal running IPython
else:
return False # Other type, if any
except NameError:
return False # Probably standard Python interpreter
@staticmethod
def _get_colorized_urdf(urdf_path, rgb, custom_mesh_search_path=None):
"""
@brief Generate a unique colorized URDF.
@remark Multiple identical URDF model of different colors can be
loaded in Gepetto-viewer this way.
@param[in] urdf_path Full path of the URDF file
@param[in] rgb RGB code defining the color of the model. It is the same for each link.
@return Full path of the colorized URDF file.
"""
color_string = "%.3f_%.3f_%.3f_1.0" % rgb
color_tag = "<color rgba=\"%.3f %.3f %.3f 1.0\"" % rgb # don't close tag with '>', in order to handle <color/> and <color></color>
colorized_tmp_path = os.path.join(
"/tmp", "colorized_urdf_rgba_" + color_string)
colorized_urdf_path = os.path.join(colorized_tmp_path,
os.path.basename(urdf_path))
if not os.path.exists(colorized_tmp_path):
os.makedirs(colorized_tmp_path)
with open(urdf_path, 'r') as urdf_file:
colorized_contents = urdf_file.read()
for mesh_fullpath in re.findall('<mesh filename="(.*)"',
colorized_contents):
colorized_mesh_fullpath = os.path.join(colorized_tmp_path,
mesh_fullpath[1:])
colorized_mesh_path = os.path.dirname(colorized_mesh_fullpath)
if not os.access(colorized_mesh_path, os.F_OK):
os.makedirs(colorized_mesh_path)
shutil.copy2(mesh_fullpath, colorized_mesh_fullpath)
colorized_contents = colorized_contents.replace(
'"' + mesh_fullpath + '"', '"' + colorized_mesh_fullpath + '"',
1)
colorized_contents = re.sub("<color rgba=\"[\d. ]*\"", color_tag,
colorized_contents)
with open(colorized_urdf_path, 'w') as colorized_urdf_file:
colorized_urdf_file.write(colorized_contents)
return colorized_urdf_path
@staticmethod
def _get_gepetto_client(open_if_needed=False):
"""
@brief Get a pointer to the running process of Gepetto-Viewer.
@details This method can be used to open a new process if necessary.
.
@param[in] open_if_needed Whether a new process must be opened if
no running process is found.
Optional: False by default
@return A pointer to the running Gepetto-viewer Client and its PID.
"""
try:
from gepetto.corbaserver.client import Client
return Client(), None
except:
try:
return Client(), None
except:
if (open_if_needed):
FNULL = open(os.devnull, 'w')
proc = subprocess.Popen(['gepetto-gui'],
shell=False,
stdout=FNULL,
stderr=FNULL)
time.sleep(1)
try:
return Client(), proc
except:
try:
return Client(), proc
except:
print("Impossible to open Gepetto-viewer")
return None, None
def _delete_gepetto_nodes_viewer(self, *nodes_path):
"""
@brief Delete a 'node' in Gepetto-viewer.
@remark Be careful, one must specify the full path of a node, including
all parent group, but without the window name, ie
'scene_name/robot_name' to delete the robot.
@param[in] nodes_path Full path of the node to delete
"""
if Viewer.backend == 'gepetto-gui':
for node_path in nodes_path:
if node_path in self._client.getNodeList():
self._client.deleteNode(node_path, True)
def _getViewerNodeName(self, geometry_object, geometry_type):
"""
@brief Get the full path of a node associated to a given geometry
object and geometry type.
@remark This is a hidden function that is not automatically imported
using 'from wdc_jiminy_py import *'. It is not intended to
be called manually.
@param[in] geometry_object Geometry object from which to get the node
@param[in] geometry_type Geometry type. It must be either
pin.GeometryType.VISUAL or pin.GeometryType.COLLISION
for display and collision, respectively.
@return Full path of the associated node.
"""
if geometry_type is pin.GeometryType.VISUAL:
return self._rb.viz.viewerVisualGroupName + '/' + geometry_object.name
elif geometry_type is pin.GeometryType.COLLISION:
return self._rb.viz.viewerCollisionGroupName + '/' + geometry_object.name
def _updateGeometryPlacements(self, visual=False):
"""
@brief Update the generalized position of a geometry object.
@remark This is a hidden function that is not automatically imported
using 'from wdc_jiminy_py import *'. It is not intended to
be called manually.
@param[in] visual Wether it is a visual or collision update
"""
if visual:
geom_model = self._rb.visual_model
geom_data = self._rb.visual_data
else:
geom_model = self._rb.collision_model
geom_data = self._rb.collision_data
pin.updateGeometryPlacements(self.pinocchio_model, self.pinocchio_data,
geom_model, geom_data)
def setCameraTransform(self, translation, rotation):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
T_pnc = np.array(translation)
T_R = SE3(R_pnc, T_pnc)
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(T_R).tolist())
else:
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def captureFrame(self):
if Viewer.backend == 'gepetto-gui':
png_path = next(tempfile._get_candidate_names())
self._client.captureFrame(self._window_id, png_path)
rgb_array = np.array(Image.open(png_path))[:, :, :-1]
os.remove(png_path)
return rgb_array
else:
raise RuntimeError(
"Screen capture through Python only available using 'gepetto-gui' backend."
)
def refresh(self):
"""
@brief Refresh the configuration of Model in the viewer.
"""
if self.use_theoretical_model:
raise RuntimeError(
"'Refresh' method only available if 'use_theoretical_model'=False."
)
if Viewer.backend == 'gepetto-gui':
if self._rb.displayCollisions:
self._client.applyConfigurations(
[self._getViewerNodeName(collision, pin.GeometryType.COLLISION)
for collision in self._rb.collision_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.collision_data.oMg[\
self._rb.collision_model.getGeometryId(collision.name)])
for collision in self._rb.collision_model.geometryObjects]
)
if self._rb.displayVisuals:
self._updateGeometryPlacements(visual=True)
self._client.applyConfigurations(
[self._getViewerNodeName(visual, pin.GeometryType.VISUAL)
for visual in self._rb.visual_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)])
for visual in self._rb.visual_model.geometryObjects]
)
self._client.refresh()
else:
self._updateGeometryPlacements(visual=True)
for visual in self._rb.visual_model.geometryObjects:
T = self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)].homogeneous.A
self._client.viewer[\
self._getViewerNodeName(visual, pin.GeometryType.VISUAL)].set_transform(T)
def display(self, evolution_robot, speed_ratio, xyz_offset=None):
t = [s.t for s in evolution_robot]
i = 0
init_time = time.time()
while i < len(evolution_robot):
s = evolution_robot[i]
if (xyz_offset is not None):
q = s.q.copy(
) # Make sure to use a copy to avoid altering the original data
q[:3] += xyz_offset
else:
q = s.q
with Viewer._lock: # It is necessary to use Lock since corbaserver does not support multiple connection simultaneously.
self._rb.display(q)
t_simu = (time.time() - init_time) * speed_ratio
i = bisect_right(t, t_simu)
if t_simu < s.t:
time.sleep(s.t - t_simu)
#-----------------------------------------------------------------------------
#---- 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)
class Viewer:
backend = None
port_forwarding = None
_backend_obj = None
_backend_exception = None
_backend_proc = None
_lock = Lock(
) # Unique threading.Lock for every simulations (in the same thread ONLY!)
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
@staticmethod
def _create_meshcat_backend():
import meshcat
from contextlib import redirect_stdout
with redirect_stdout(None):
Viewer._backend_obj = meshcat.Visualizer()
Viewer._backend_proc = Viewer._backend_obj.window.server_proc
@staticmethod
def reset_port_forwarding(port_forwarding=None):
Viewer.port_forwarding = port_forwarding
@staticmethod
def display_jupyter_cell(height=600,
width=900,
force_create_backend=False):
if Viewer.backend == 'meshcat' and Viewer._is_notebook():
from IPython.core.display import HTML, display as ipython_display
if Viewer._backend_obj is None:
if force_create_backend:
Viewer._create_meshcat_backend()
else:
raise ValueError(
"No meshcat backend available and 'force_create_backend' is set to False."
)
viewer_url = Viewer._backend_obj.url()
if Viewer.port_forwarding is not None:
url_port_pattern = '(?<=:)[0-9]+(?=/)'
port_localhost = int(
re.search(url_port_pattern, viewer_url).group())
if port_localhost in Viewer.port_forwarding.keys():
viewer_url = re.sub(
url_port_pattern,
str(Viewer.port_forwarding[port_localhost]),
viewer_url)
else:
print(
"Port forwarding defined but no port mapping associated with {port_localhost}."
)
jupyter_html = f'\n<div style="height: {height}px; width: {width}px; overflow-x: auto; overflow-y: hidden; resize: both">\
\n<iframe src="{viewer_url}" style="width: 100%; height: 100%; border: none">\
</iframe>\n</div>\n'
ipython_display(HTML(jupyter_html))
else:
raise ValueError(
"Display in a Jupyter cell is only available using 'meshcat' backend and within a Jupyter notebook."
)
@staticmethod
def close():
if Viewer._backend_proc is not None:
if Viewer._backend_proc.poll() is None:
Viewer._backend_proc.terminate()
Viewer._backend_proc = None
Viewer._backend_obj = None
Viewer._backend_exception = None
@staticmethod
def _is_notebook():
try:
shell = get_ipython().__class__.__name__
if shell == 'ZMQInteractiveShell':
return True # Jupyter notebook or qtconsole
elif shell == 'TerminalInteractiveShell':
return False # Terminal running IPython
else:
return False # Other type, if any
except NameError:
return False # Probably standard Python interpreter
@staticmethod
def _get_colorized_urdf(urdf_path, rgb, root_path=None):
"""
@brief Generate a unique colorized URDF.
@remark Multiple identical URDF model of different colors can be
loaded in Gepetto-viewer this way.
@param[in] urdf_path Full path of the URDF file
@param[in] rgb RGB code defining the color of the model. It is the same for each link.
@param[in] root_path Root path of the meshes.
@return Full path of the colorized URDF file.
"""
color_string = "%.3f_%.3f_%.3f_1.0" % rgb
color_tag = "<color rgba=\"%.3f %.3f %.3f 1.0\"" % rgb # don't close tag with '>', in order to handle <color/> and <color></color>
colorized_tmp_path = os.path.join(
tempfile.gettempdir(), "colorized_urdf_rgba_" + color_string)
colorized_urdf_path = os.path.join(colorized_tmp_path,
os.path.basename(urdf_path))
if not os.path.exists(colorized_tmp_path):
os.makedirs(colorized_tmp_path)
with open(urdf_path, 'r') as urdf_file:
colorized_contents = urdf_file.read()
for mesh_fullpath in re.findall('<mesh filename="(.*)"',
colorized_contents):
# Replace package path by root_path.
if mesh_fullpath.startswith('package://'):
mesh_fullpath = root_path + mesh_fullpath[len("package:/"):]
colorized_mesh_fullpath = os.path.join(colorized_tmp_path,
mesh_fullpath[1:])
colorized_mesh_path = os.path.dirname(colorized_mesh_fullpath)
if not os.access(colorized_mesh_path, os.F_OK):
os.makedirs(colorized_mesh_path)
shutil.copy2(mesh_fullpath, colorized_mesh_fullpath)
colorized_contents = colorized_contents.replace(
'"' + mesh_fullpath + '"', '"' + colorized_mesh_fullpath + '"',
1)
colorized_contents = re.sub("<color rgba=\"[\d. ]*\"", color_tag,
colorized_contents)
with open(colorized_urdf_path, 'w') as colorized_urdf_file:
colorized_urdf_file.write(colorized_contents)
return colorized_urdf_path
@staticmethod
def _get_gepetto_client(create_if_needed=False, create_timeout=2000):
"""
@brief Get a pointer to the running process of Gepetto-Viewer.
@details This method can be used to open a new process if necessary.
.
@param[in] create_if_needed Whether a new process must be created if
no running process is found.
Optional: False by default
@param[in] create_timeout Wait some millisecond before considering
creating new viewer as failed.
Optional: 1s by default
@return A pointer to the running Gepetto-viewer Client and its PID.
"""
from gepetto.corbaserver.client import Client as gepetto_client
try:
# Get the existing Gepetto client
client = gepetto_client()
# Try to fetch the list of scenes to make sure that the Gepetto client is responding
client.gui.getSceneList()
return client, None
except:
try:
client = gepetto_client()
client.gui.getSceneList()
return client, None
except:
if (create_if_needed):
FNULL = open(os.devnull, 'w')
proc = subprocess.Popen(
['/opt/openrobots/bin/gepetto-gui'],
shell=False,
stdout=FNULL,
stderr=FNULL)
for _ in range(max(2, int(
create_timeout /
200))): # Must try at least twice for robustness
time.sleep(0.2)
try:
return gepetto_client(), proc
except:
pass
print("Impossible to open Gepetto-viewer")
return None, None
def _delete_gepetto_nodes_viewer(self, *nodes_path):
"""
@brief Delete a 'node' in Gepetto-viewer.
@remark Be careful, one must specify the full path of a node, including
all parent group, but without the window name, ie
'scene_name/robot_name' to delete the robot.
@param[in] nodes_path Full path of the node to delete
"""
if Viewer.backend == 'gepetto-gui':
for node_path in nodes_path:
if node_path in self._client.getNodeList():
self._client.deleteNode(node_path, True)
def _getViewerNodeName(self, geometry_object, geometry_type):
"""
@brief Get the full path of a node associated with a given geometry
object and geometry type.
@remark This is a hidden function not intended to be called manually.
@param[in] geometry_object Geometry object from which to get the node
@param[in] geometry_type Geometry type. It must be either
pin.GeometryType.VISUAL or pin.GeometryType.COLLISION
for display and collision, respectively.
@return Full path of the associated node.
"""
if geometry_type is pin.GeometryType.VISUAL:
return self._rb.viz.viewerVisualGroupName + '/' + geometry_object.name
elif geometry_type is pin.GeometryType.COLLISION:
return self._rb.viz.viewerCollisionGroupName + '/' + geometry_object.name
def _updateGeometryPlacements(self, visual=False):
"""
@brief Update the generalized position of a geometry object.
@remark This is a hidden function not intended to be called manually.
@param[in] visual Wether it is a visual or collision update
"""
if visual:
geom_model = self._rb.visual_model
geom_data = self._rb.visual_data
else:
geom_model = self._rb.collision_model
geom_data = self._rb.collision_data
pin.updateGeometryPlacements(self.pinocchio_model, self.pinocchio_data,
geom_model, geom_data)
def setCameraTransform(self,
translation=np.zeros(3),
rotation=np.zeros(3),
relative=False):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
H_abs = SE3(R_pnc, np.array(translation))
if relative:
H_orig = XYZQUATToSe3(
self._client.getCameraTransform(self._window_id))
H_abs = H_abs * H_orig
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(H_abs).tolist())
else:
if relative:
raise RuntimeError(
"'relative'=True not available with meshcat.")
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def captureFrame(self):
if Viewer.backend == 'gepetto-gui':
png_path = next(tempfile._get_candidate_names())
self._client.captureFrame(self._window_id, png_path)
rgb_array = np.array(Image.open(png_path))[:, :, :-1]
os.remove(png_path)
return rgb_array
else:
raise RuntimeError(
"Screen capture through Python only available using 'gepetto-gui' backend."
)
def refresh(self):
"""
@brief Refresh the configuration of Robot in the viewer.
"""
if self.use_theoretical_model:
raise RuntimeError(
"'Refresh' method only available if 'use_theoretical_model'=False."
)
if Viewer.backend == 'gepetto-gui':
with self._lock:
if self._rb.displayCollisions:
self._client.applyConfigurations(
[self._getViewerNodeName(collision, pin.GeometryType.COLLISION)
for collision in self._rb.collision_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.collision_data.oMg[\
self._rb.collision_model.getGeometryId(collision.name)])
for collision in self._rb.collision_model.geometryObjects]
)
if self._rb.displayVisuals:
self._updateGeometryPlacements(visual=True)
self._client.applyConfigurations(
[self._getViewerNodeName(visual, pin.GeometryType.VISUAL)
for visual in self._rb.visual_model.geometryObjects],
[pin.se3ToXYZQUATtuple(self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)])
for visual in self._rb.visual_model.geometryObjects]
)
self._client.refresh()
else:
self._updateGeometryPlacements(visual=True)
for visual in self._rb.visual_model.geometryObjects:
T = self._rb.visual_data.oMg[\
self._rb.visual_model.getGeometryId(visual.name)].homogeneous
self._client.viewer[\
self._getViewerNodeName(visual, pin.GeometryType.VISUAL)].set_transform(T)
def display(self, evolution_robot, replay_speed, xyz_offset=None):
t = [s.t for s in evolution_robot]
i = 0
init_time = time.time()
while i < len(evolution_robot):
s = evolution_robot[i]
if (xyz_offset is not None):
q = s.q.copy(
) # Make sure to use a copy to avoid altering the original data
q[:3] += xyz_offset
else:
q = s.q
try:
with self._lock:
self._rb.display(q)
except Viewer._backend_exception:
break
t_simu = (time.time() - init_time) * replay_speed
i = bisect_right(t, t_simu)
sleep(s.t - t_simu)
class CallbackLogger:
def __init__(self, sleeptime=1e-2):
"""
nfevl: iteration number
dt: animation time pause
"""
self.nfeval = 1
self.dt = sleeptime
def __call__(self, q):
print '===CBK=== {0:4d} {1: 3.2f} {2: 3.2f}'.format(
self.nfeval, q[0], q[1])
robot.display(q)
place('world/blue', robot.position(q, 6))
self.nfeval += 1
time.sleep(self.dt)
## Question 1-2-3
target = np.matrix([0.5, 0.1, 0.2]).T # x,y,z
place('world/red', pinocchio.SE3(eye(3), target))
cost = Cost(target)
robot.display(robot.q0)
time.sleep(1)
print 'Let s go to pdes.'
def go():
qopt = fmin_bfgs(cost, robot.q0, callback=CallbackLogger(.5))
return qopt
)
#-----------------------------------------------------------------------------
#---- 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=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)
-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)
)
#-----------------------------------------------------------------------------
#---- 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)
# DEFINE POSTURE TASK
def display(self, q, refresh=True):
self.q_def[NQ_OFFSET:] = q.reshape(self.nq - NQ_OFFSET)
return RobotWrapper.display(self, self.q_def)
qInit = np.array([0.59346963, 0.25128594, 0.62805152, 0.17837691, 0.86218068, 0.3577253])
qInit = np.asmatrix(qInit).T
se3.forwardKinematics(robot.model, robot.data, qInit)
visualObj = robot.visual_model.geometryObjects[4]
visualName = visualObj.name
visualRef = robot.getViewerNodeName(visualObj, se3.GeometryType.VISUAL)
rgbt = [1.0, 1.0, 1.0, 1.0]
robot.viewer.gui.addSphere("world/sphere", .1, rgbt)
qSphere = [0.5, .1, .2, 1.0, 0, 0, 0]
robot.viewer.gui.applyConfiguration("world/sphere", qSphere)
robot.viewer.gui.refresh()
robot.display(qInit)
#PyIpopt stuff
timeSteps = 3
dt = .01
nvar = robot.nv*timeSteps
ncon = 1
#q_ref1 = np.array([-1.49411011e-02,-1.32003896e+00,2.09324591e+00,-7.73206945e-01,-1.49411010e-02,-2.32376275e-09])
q_ref1 = np.array([0.59346963,0.25128594,0.62805152,0.17837691,0.86218068,0.3577253])
q_ref1 = np.asmatrix(q_ref1).T
qRef = np.empty([timeSteps, 1, robot.nq, 1])
temp = 0
