Commit eacfd6e8 authored by jcarpent's avatar jcarpent
Browse files

[Doc] Fix bug in documentation of frameJacobian

parent 73d71411
#
# Copyright (c) 2015-2017 CNRS
#
# This file is part of Pinocchio
# Pinocchio is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
# Pinocchio is distributed in the hope that it will be
# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# Pinocchio If not, see
# <http://www.gnu.org/licenses/>.
from . import libpinocchio_pywrap as se3
from . import utils
import time
import os
class RobotWrapper(object):
def initFromURDF(self,filename, package_dirs=None, root_joint=None, verbose=False):
if root_joint is None:
model = se3.buildModelFromUrdf(filename)
else:
model = se3.buildModelFromUrdf(filename, root_joint)
if "buildGeomFromUrdf" not in dir(se3):
collision_model = None
visual_model = None
if verbose:
print('Info: the Geometry Module has not been compiled with Pinocchio. No geometry model and data have been built.')
else:
if package_dirs is None:
self.collision_model = se3.buildGeomFromUrdf(self.model, filename, se3.GeometryType.COLLISION)
self.visual_model = se3.buildGeomFromUrdf(self.model, filename, se3.GeometryType.VISUAL)
else:
if not all(isinstance(item, str) for item in package_dirs):
raise Exception('The list of package directories is wrong. At least one is not a string')
else:
self.collision_model = se3.buildGeomFromUrdf(model, filename,
utils.fromListToVectorOfString(package_dirs), se3.GeometryType.COLLISION)
self.visual_model = se3.buildGeomFromUrdf(model, filename,
utils.fromListToVectorOfString(package_dirs), se3.GeometryType.VISUAL)
RobotWrapper.__init__(self,model=model,collision_model=collision_model,visual_model=visual_model)
def __init__(self, model = se3.Model(), collision_model = None, visual_model = None, verbose=False):
self.model = model
self.data = self.model.createData()
self.collision_model = collision_model
self.visual_model = visual_model
if "buildGeomFromUrdf" not in dir(se3):
self.collision_data = None
self.visual_data = None
if verbose:
print 'Info: the Geometry Module has not been compiled with Pinocchio. No geometry model and data have been built.'
else:
if self.collision_model is None:
self.collision_data = None
else:
self.collision_data = se3.GeometryData(self.collision_model)
if self.visual_model is None:
self.visual_data = None
else:
self.visual_data = se3.GeometryData(self.visual_model)
self.v0 = utils.zero(self.nv)
self.q0 = self.model.neutralConfiguration
def increment(self, q, dq):
q_next = se3.integrate(self.model,q,dq)
q[:] = q_next[:]
@property
def nq(self):
return self.model.nq
@property
def nv(self):
return self.model.nv
def com(self, q=None, v=None, a=None):
if q is None:
se3.centerOfMass(self.model, self.data)
return data.com[0]
if v is not None:
if a is None:
se3.centerOfMass(self.model, self.data, q, v)
return self.data.com[0], self.data.vcom[0]
se3.centerOfMass(self.model, self.data, q, v, a)
return self.data.com[0], self.data.vcom[0], self.data.acom[0]
return se3.centerOfMass(self.model, self.data, q)
def Jcom(self, q):
return se3.jacobianCenterOfMass(self.model, self.data, q)
def mass(self, q):
return se3.crba(self.model, self.data, q)
def bias(self, q, v):
return se3.nle(self.model, self.data, q, v)
def gravity(self, q):
return se3.rnea(self.model, self.data, q, self.v0, self.v0)
def forwardKinematics(self, q, v=None, a=None):
if v is not None:
if a is not None:
se3.forwardKinematics(self.model, self.data, q, v, a)
else:
se3.forwardKinematics(self.model, self.data, q, v)
else:
se3.forwardKinematics(self.model, self.data, q)
def position(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q)
return self.data.oMi[index]
def velocity(self, q, v, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v)
return self.data.v[index]
def acceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a)
return self.data.a[index]
def framePosition(self, q, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q)
frame = self.model.frames[index]
parentPos = self.data.oMi[frame.parent]
return parentPos.act(frame.placement)
def frameVelocity(self, q, v, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v)
frame = self.model.frames[index]
parentJointVel = self.data.v[frame.parent]
return frame.placement.actInv(parentJointVel)
def frameAcceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a)
frame = self.model.frames[index]
parentJointAcc = self.data.a[frame.parent]
return frame.placement.actInv(parentJointAcc)
def frameClassicAcceleration(self, q, v, a, index, update_kinematics=True):
if update_kinematics:
se3.forwardKinematics(self.model, self.data, q, v, a)
f = self.model.frames[index]
af = f.placement.actInv(self.data.a[f.parent])
vf = f.placement.actInv(self.data.v[f.parent])
af.linear += np.cross(vf.angular.T, vf.linear.T).T
return af;
def jacobian(self, q, index, update_kinematics=True, local_frame=True):
return se3.jacobian(self.model, self.data, q, index, local_frame, update_kinematics)
def computeJacobians(self, q):
return se3.computeJacobians(self.model, self.data, q)
def updateGeometryPlacements(self, q=None, visual=False):
if visual:
geom_model = self.visual_model
geom_data = self.visual_data
else:
geom_model = self.collision_model
geom_data = self.collision_data
if q is not None:
se3.updateGeometryPlacements(self.model, self.data, geom_model, geom_data, q)
else:
se3.updateGeometryPlacements(self.model, self.data, geom_model, geom_data)
def framesKinematics(self, q):
se3.framesKinematics(self.model, self.data, q)
def framePosition(self, index):
f = self.model.frames[index]
return self.data.oMi[f.parent].act(f.placement)
def frameVelocity(self, index):
f = self.model.frames[index]
return f.placement.actInv(self.data.v[f.parent])
''' Return the spatial acceleration of the specified frame. '''
def frameAcceleration(self, index):
f = self.model.frames[index]
return f.placement.actInv(self.data.a[f.parent])
def frameClassicAcceleration(self, index):
f = self.model.frames[index]
a = f.placement.actInv(self.data.a[f.parent])
v = f.placement.actInv(self.data.v[f.parent])
a.linear += np.cross(v.angular.T, v.linear.T).T
return a;
''' Call computeJacobians if update_geometry is true. If not, user should call computeJacobians first.
Then call getJacobian and return the resulted jacobian matrix. Attention: if update_geometry is true,
the function computes all the jacobians of the model. It is therefore outrageously costly wrt a
dedicated call. Use only with update_geometry for prototyping.
'''
def frameJacobian(self, q, index, update_geometry=True, local_frame=True):
return se3.frameJacobian(self.model, self.data, q, index, local_frame, update_geometry)
def framesKinematics(self, q):
se3.framesKinematics(self.model, self.data, q)
''' Call computeJacobians if update_geometry is true. If not, user should call computeJacobians first.
Then call getJacobian and return the resulted jacobian matrix. Attention: if update_geometry is true,
the function computes all the jacobians of the model. It is therefore outrageously costly wrt a
dedicated call. Use only with update_geometry for prototyping.
'''
def frameJacobian(self, q, index, update_geometry=True, local_frame=True):
return se3.frameJacobian(self.model, self.data, q, index, local_frame, update_geometry)
# --- ACCESS TO NAMES ----
# Return the index of the joint whose name is given in argument.
def index(self, name):
return [i for i, n in enumerate(self.model.names) if n == name][0]
# --- VIEWER ---
# For each geometry object, returns the corresponding name of the node in Gepetto-viewer.
def getViewerNodeName(self, geometry_object, geometry_type):
if geometry_type is se3.GeometryType.VISUAL:
return self.viewerVisualGroupName + '/' + geometry_object.name
elif geometry_type is se3.GeometryType.COLLISION:
return self.viewerCollisionGroupName + '/' + geometry_object.name
def initDisplay(self, windowName="python-pinocchio", sceneName="world", loadModel=False):
"""
Init gepetto-viewer by loading the gui and creating a window.
"""
import gepetto.corbaserver
try:
self.viewer = gepetto.corbaserver.Client()
gui = self.viewer.gui
# Create window
window_l = gui.getWindowList()
if not windowName in window_l:
self.windowID = self.viewer.gui.createWindow(windowName)
else:
self.windowID = self.viewer.gui.getWindowID(windowName)
# Create scene if needed
scene_l = gui.getSceneList()
if sceneName not in scene_l:
gui.createScene(sceneName)
self.sceneName = sceneName
gui.addSceneToWindow(sceneName, self.windowID)
if loadModel:
self.loadDisplayModel()
except:
print("Error while starting the viewer client. ")
print("Check wheter gepetto-viewer is properly started")
# Create the scene displaying the robot meshes in gepetto-viewer
def loadDisplayModel(self, rootNodeName="pinocchio"):
def loadDisplayGeometryObject(geometry_object,geometry_type):
from .rpy import npToTuple
meshName = self.getViewerNodeName(geometry_object,geometry_type)
meshPath = geometry_object.meshPath
meshTexturePath = geometry_object.meshTexturePath
meshScale = geometry_object.meshScale
meshColor = geometry_object.meshColor
if gui.addMesh(meshName, meshPath):
gui.setScale(meshName, npToTuple(meshScale))
if geometry_object.overrideMaterial:
gui.setColor(meshName, npToTuple(meshColor))
if meshTexturePath is not '':
gui.setTexture(meshName, meshTexturePath)
# Start a new "scene" in this window, named "world", with just a floor.
gui = self.viewer.gui
self.viewerRootNodeName = self.sceneName + "/" + rootNodeName
if not gui.nodeExists(self.viewerRootNodeName):
gui.createGroup(self.viewerRootNodeName)
self.viewerCollisionGroupName = self.viewerRootNodeName + "/" + "collisions"
if not gui.nodeExists(self.viewerCollisionGroupName):
gui.createGroup(self.viewerCollisionGroupName)
self.viewerVisualGroupName = self.viewerRootNodeName + "/" + "visuals"
if not gui.nodeExists(self.viewerVisualGroupName):
gui.createGroup(self.viewerVisualGroupName)
# iterate over visuals and create the meshes in the viewer
for collision in self.collision_model.geometryObjects:
loadDisplayGeometryObject(collision,se3.GeometryType.COLLISION)
self.displayCollisions(False)
for visual in self.visual_model.geometryObjects:
loadDisplayGeometryObject(visual,se3.GeometryType.VISUAL)
self.displayVisuals(True)
# Finally, refresh the layout to obtain your first rendering.
gui.refresh()
# Display in gepetto-view the robot at configuration q, by placing all the bodies.
def display(self, q):
if 'viewer' not in self.__dict__:
return
gui = self.viewer.gui
# Update the robot kinematics and geometry.
self.forwardKinematics(q)
if self.display_collisions:
self.updateGeometryPlacements(visual=False)
for collision in self.collision_model.geometryObjects:
M = self.visual_data.oMg[self.collision_model.getGeometryId(collision.name)]
conf = utils.se3ToXYZQUAT(M)
gui.applyConfiguration(self.getViewerNodeName(collision,se3.GeometryType.COLLISION), conf)
if self.display_visuals:
self.updateGeometryPlacements(visual=True)
for visual in self.visual_model.geometryObjects:
M = self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]
conf = utils.se3ToXYZQUAT(M)
gui.applyConfiguration(self.getViewerNodeName(visual,se3.GeometryType.VISUAL), conf)
gui.refresh()
def displayCollisions(self,visibility):
gui = self.viewer.gui
self.display_collisions = visibility
if visibility:
visibility_mode = "ON"
else:
visibility_mode = "OFF"
for collision in self.collision_model.geometryObjects:
nodeName = self.getViewerNodeName(collision,se3.GeometryType.COLLISION)
gui.setVisibility(nodeName,visibility_mode)
def displayVisuals(self,visibility):
gui = self.viewer.gui
self.display_visuals = visibility
if visibility:
visibility_mode = "ON"
else:
visibility_mode = "OFF"
for visual in self.visual_model.geometryObjects:
nodeName = self.getViewerNodeName(visual,se3.GeometryType.VISUAL)
gui.setVisibility(nodeName,visibility_mode)
def play(self, q_trajectory, dt):
for k in range(q_trajectory.shape[1]):
t0 = time.time()
self.display(q_trajectory[:, k])
t1 = time.time()
elapsed_time = t1 - t0
if elapsed_time < dt:
time.sleep(dt - elapsed_time)
__all__ = ['RobotWrapper']
......@@ -69,8 +69,8 @@ namespace se3
(Data::Matrix6x (*)(const Model &, Data &, const Model::FrameIndex, const bool, const Eigen::VectorXd &))&frame_jacobian_proxy,
bp::args("Model","Data",
"Operational frame ID (int)",
"Configuration q (size Model::nq)",
"frame (true = local, false = world)"),
"frame (true = local, false = world)",
"Configuration q (size Model::nq)"),
"Compute the Jacobian of the frame given by its ID either in the local or the world frames."
"The columns of the Jacobian are expressed in the frame coordinates.\n"
"In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v,"
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment