Commit a71c0449 authored by fvalenza's avatar fvalenza
Browse files

Merge pull request #9 from fvalenza/master

Aggregating pull request of Thomas to handle fixed joint when parsing urdf file

    dimension of q is still minimal
    inertias are merged with the closest the movable parent
    placements of fixed join are recorded in model to send it to gepetto-viewer
    COM and RNEA have been tested locally - unit-test to come.
parents 21bcff10 1030c9ba
......@@ -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;
......
......@@ -14,7 +14,6 @@ namespace urdf
typedef boost::shared_ptr<const Joint> JointConstPtr;
typedef boost::shared_ptr<const Link> LinkConstPtr;
typedef boost::shared_ptr<Link> LinkPtr;
typedef boost::shared_ptr<const Inertial> InertialConstPtr;
}
......@@ -22,7 +21,6 @@ namespace se3
{
namespace urdf
{
Inertia convertFromUrdf( const ::urdf::Inertial& Y )
{
const ::urdf::Vector3 & p = Y.origin.position;
......@@ -34,7 +32,6 @@ namespace se3
Eigen::Matrix3d I; I << Y.ixx,Y.ixy,Y.ixz
, Y.ixy,Y.iyy,Y.iyz
, Y.ixz,Y.iyz,Y.izz;
return Inertia(Y.mass,com,R*I*R.transpose());
}
......@@ -58,20 +55,24 @@ namespace se3
return AXIS_UNALIGNED;
}
void parseTree( ::urdf::LinkConstPtr link, Model & model, bool freeFlyer )
void parseTree( ::urdf::LinkConstPtr link, Model & model, bool freeFlyer, SE3 placementOffset = SE3::Identity() )
{
::urdf::JointConstPtr joint = link->parent_joint;
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;
//assert(link->inertial && "The parser cannot accept trivial mass");
const Inertia & Y = (link->inertial) ?
convertFromUrdf(*link->inertial)
: Inertia::Identity();
//std::cout << "Inertia: " << Y << std::endl;
// std::cout << "placementOffset: " << placementOffset << std::endl;
bool visual = (link->visual) ? true : false;
......@@ -85,7 +86,7 @@ namespace se3
: model.getBodyId( link->getParent()->parent_joint->name );
//std::cout << joint->name << " === " << parent << std::endl;
const SE3 & jointPlacement = convertFromUrdf(joint->parent_to_joint_origin_transform);
const SE3 & jointPlacement = placementOffset*convertFromUrdf(joint->parent_to_joint_origin_transform);
//std::cout << "Parent = " << parent << std::endl;
//std::cout << "Placement = " << (Matrix4)jointPlacement << std::endl;
......@@ -140,35 +141,52 @@ namespace se3
assert(false && "Only X, Y or Z axis are accepted." );
break;
default:
assert( false && "Fatal Error while extracting revolute joint axis");
assert( false && "Fatal Error while extracting prismatic joint axis");
break;
}
break;
}
case ::urdf::Joint::FIXED:
{
model.mergeFixedBody(parent, jointPlacement, Y);
assert( (link->child_links.empty()) && "Fixed body has joint child. Shouldn't happen");
// For the moment, fixed bodies are handled only if end of chain (and only once)
break;
// 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
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
}
break;
}
default:
{
std::cerr << "The joint type " << joint->type << " is not supported." << std::endl;
assert(false && "Only revolute joint are accepted." );
assert(false && "Only revolute, prismatic and fixed joints are accepted." );
break;
}
}
}
else if(freeFlyer)
{ /* The link is the root of the body. */
{ // The link is the root of the body.
model.addBody( 0, JointModelFreeFlyer(), SE3::Identity(), Y, "root", link->name, true );
}
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
BOOST_FOREACH(::urdf::LinkConstPtr child,link->child_links)
{
parseTree( child,model,freeFlyer );
parseTree( child,model,freeFlyer,nextPlacementOffset );
}
}
......
......@@ -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