Commit 697d16aa authored by Thomas Flayols's avatar Thomas Flayols Committed by Valenza Florian
Browse files

[Major][C++][Python] Save merged fixed-bodies in model to display them in gepetto-viewer

Add attributes in model.hpp
	- names of fixed body
	- indexes of their closer movable parent
	- visuals booleans
	- number of fixed body
modify display method in robot_wrapper
	- compute and send the absolute placement from relative to the closer movable parent
parent d450fed4
......@@ -38,6 +38,12 @@ namespace se3
std::vector<std::string> bodyNames;// Name of the body attached to the output of joint <i>
std::vector<bool> hasVisual; // True iff body <i> has a visual mesh.
int nFixBody; // Number of fixed-bodies (= number of fixed-joints)
std::vector<SE3> fix_lmpMi; // Fixed-body relative placement (wrt last moving parent)
std::vector<Model::Index> fix_lastMovingParent; // Fixed-body index of the last moving parent
std::vector<bool> fix_hasVisual; // True iff fixed-body <i> has a visual mesh.
std::vector<std::string> fix_bodyNames;// Name of fixed-joint <i>
Motion gravity; // Spatial gravity
static const Eigen::Vector3d gravity981; // Default 3D gravity (=(0,0,9.81))
......@@ -45,6 +51,7 @@ namespace se3
: nq(0)
, nv(0)
, nbody(1)
, nFixBody(0)
, inertias(1)
, jointPlacements(1)
, joints(1)
......@@ -62,6 +69,10 @@ namespace se3
Index addBody( Index parent,const JointModelBase<D> & j,const SE3 & placement,
const Inertia & Y, const std::string & jointName = "",
const std::string & bodyName = "", bool visual = false );
Index addFixedBody( Index fix_lastMovingParent,
const SE3 & placementFromLastMoving,
const std::string &jointName = "",
bool visual=false);
void mergeFixedBody(Index parent, const SE3 & placement, const Inertia & Y);
Index getBodyId( const std::string & name ) const;
bool existBodyName( const std::string & name ) const;
......@@ -105,12 +116,12 @@ namespace se3
std::vector<Eigen::Vector3d> com; // Subtree com position.
std::vector<double> mass; // Subtree total mass.
Eigen::Matrix<double,3,Eigen::Dynamic> Jcom; // Jacobian of center of mass.
Data( const Model& ref );
private:
void computeLastChild(const Model& model);
void computeParents_fromRow(const Model& model);
};
const Eigen::Vector3d Model::gravity981 (0,0,-9.81);
......@@ -174,6 +185,20 @@ namespace se3
return idx;
}
Model::Index Model::addFixedBody( Index lastMovingParent,
const SE3 & placementFromLastMoving,
const std::string & bodyName,
bool visual )
{
Index idx = nFixBody++;
fix_lastMovingParent.push_back(lastMovingParent);
fix_lmpMi .push_back(placementFromLastMoving);
fix_hasVisual .push_back(visual);
fix_bodyNames .push_back( (bodyName!="")?bodyName:random(8));
return idx;
}
void Model::mergeFixedBody(Index parent, const SE3 & placement, const Inertia & Y)
{
const Inertia & iYf = Y.se3Action(placement); //TODO
......@@ -271,7 +296,7 @@ namespace se3
else parents_fromRow[idx_vj] = -1;
nvSubtree_fromRow[idx_vj] = nvSubtree[joint];
for( int row=1;row<nvj;++row)
for( int row=1;row<nvj;++row)
{
parents_fromRow[idx_vj+row] = idx_vj+row-1;
nvSubtree_fromRow[idx_vj+row] = nvSubtree[joint]-row;
......
......@@ -59,14 +59,13 @@ namespace se3
{
::urdf::JointConstPtr joint = link->parent_joint;
SE3 nextPlacementOffset=SE3::Identity(); // in most cases, no offset for the next link
SE3 nextPlacementOffset = SE3::Identity(); // OffSet of the next link. In case we encounter a fixed joint, we need to propagate the length of its attached body to next joint.
// std::cout << " *** " << link->name << " < attached by joint ";
// if(joint)
// std::cout << "#" << link->parent_joint->name << std::endl;
// else std::cout << "###ROOT" << std::endl;
//std::cout << " *** " << link->name << " < attached by joint ";
//assert(link->inertial && "The parser cannot accept trivial mass");
const Inertia & Y = (link->inertial) ?
......@@ -149,14 +148,21 @@ namespace se3
}
case ::urdf::Joint::FIXED:
{
/* In case of fixed join: -add the inertia of the link to his parent in the model
* -let all the children become children of parent
* -inform the parser of the offset to apply
* */
// In case of fixed join:
// -add the inertia of the link to his parent in the model
// -let all the children become children of parent
// -inform the parser of the offset to apply
// -add fixed body in model to display it in gepetto-viewer
model.mergeFixedBody(parent, jointPlacement, Y); //Modify the parent inertia in the model
SE3 ptjot_se3 = convertFromUrdf(link->parent_joint->parent_to_joint_origin_transform);
//transformation of the current placement offset (important if several fixed join following)
nextPlacementOffset=placementOffset*ptjot_se3;
//transformation of the current placement offset
nextPlacementOffset = placementOffset*ptjot_se3;
//add the fixed Body in the model for the viewer
model.addFixedBody(parent,nextPlacementOffset,link->name,visual);
BOOST_FOREACH(::urdf::LinkPtr child_link,link->child_links)
{
child_link->setParent(link->getParent() ); //skip the fixed generation
......
......@@ -70,8 +70,14 @@ namespace se3
.add_property("hasVisual",
bp::make_function(&ModelPythonVisitor::hasVisual,
bp::return_internal_reference<>()) )
.add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
.add_property("nFixBody", &ModelPythonVisitor::nFixBody)
.add_property("fix_lmpMi", bp::make_function(&ModelPythonVisitor::fix_lmpMi, bp::return_internal_reference<>()) )
.add_property("fix_lastMovingParent",bp::make_function(&ModelPythonVisitor::fix_lastMovingParent,bp::return_internal_reference<>()) )
.add_property("fix_hasVisual", bp::make_function(&ModelPythonVisitor::fix_hasVisual, bp::return_internal_reference<>()) )
.add_property("fix_bodyNames", bp::make_function(&ModelPythonVisitor::fix_bodyNames, bp::return_internal_reference<>()) )
.add_property("gravity",&ModelPythonVisitor::gravity,&ModelPythonVisitor::setGravity)
.def("BuildEmptyModel",&ModelPythonVisitor::maker_empty)
.staticmethod("BuildEmptyModel")
.def("BuildHumanoidSimple",&ModelPythonVisitor::maker_humanoidSimple)
......@@ -93,9 +99,17 @@ namespace se3
static std::vector<std::string> & names ( ModelHandler & m ) { return m->names; }
static std::vector<std::string> & bodyNames ( ModelHandler & m ) { return m->bodyNames; }
static std::vector<bool> & hasVisual ( ModelHandler & m ) { return m->hasVisual; }
static int nFixBody( ModelHandler & m ) { return m->nFixBody; }
static std::vector<SE3> & fix_lmpMi ( ModelHandler & m ) { return m->fix_lmpMi; }
static std::vector<Model::Index> & fix_lastMovingParent( ModelHandler & m ) { return m->fix_lastMovingParent; }
static std::vector<bool> & fix_hasVisual ( ModelHandler & m ) { return m->fix_hasVisual; }
static std::vector<std::string> & fix_bodyNames ( ModelHandler & m ) { return m->fix_bodyNames; }
static Motion gravity( ModelHandler & m ) { return m->gravity; }
static void setGravity( ModelHandler & m,const Motion_fx & g ) { m->gravity = g; }
static ModelHandler maker_empty()
{
return ModelHandler( new Model(),true );
......
......@@ -58,6 +58,11 @@ class RobotWrapper:
assert( self.model.hasVisual[index] )
return self.viewerRootNodeName+'/'+self.model.bodyNames[index]
def viewerFixedNodeNames(self,index):
assert( self.model.fix_hasVisual[index] )
return self.viewerRootNodeName+'/'+self.model.fix_bodyNames[index]
def initDisplay(self,viewerRootNodeName = "world/pinocchio", loadModel = False):
import gepetto.corbaserver
try:
......@@ -92,12 +97,20 @@ class RobotWrapper:
if 'viewer' not in self.__dict__: return
# Update the robot geometry.
se3.kinematics(self.model,self.data,q,self.v0)
# Iteratively place the robot bodies.
# Iteratively place the moving robot bodies.
for i in range(1,self.model.nbody):
if self.model.hasVisual[i]:
M = self.data.oMi[i]
self.viewer.gui.applyConfiguration(self.viewerNodeNames(i),
utils.se3ToXYZQUAT(M))
# Iteratively place the fixed robot bodies.
for i in range(0,self.model.nFixBody):
if self.model.fix_hasVisual[i]:
index_last_movable=self.model.fix_lastMovingParent[i]
oMlmp = self.data.oMi[index_last_movable]
lmpMi = self.model.fix_lmpMi[i]
M = oMlmp * lmpMi
self.viewer.gui.applyConfiguration(self.viewerFixedNodeNames(i),utils.se3ToXYZQUAT(M))
self.viewer.gui.refresh()
__all__ = [ 'RobotWrapper' ]
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