Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow to attach a robot at a specified frame. #91

Merged
merged 5 commits into from
May 18, 2019
+98 −76
Merged

Allow to attach a robot at a specified frame. #91

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
dcb8e15
Fix API of loadModel (use base frame rather than base joint)
jmirabel Apr 15, 2019
91bc9b3
Change default bounding volume to BV_OBB rather than BV_OBBRSS
jmirabel Apr 15, 2019
58118f0
Make MeshLoader static to avoid reloading the same mesh several times.
jmirabel Apr 15, 2019
11c028c
Fix bugs.
jmirabel Apr 16, 2019
b463f02
Update Frame class.
jmirabel Apr 19, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ SEARCH_FOR_BOOST()

# Search for dependecies.
ADD_REQUIRED_DEPENDENCY("hpp-util >= 3.2")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.0")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.1.2")
ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.2.92")

ADD_OPTIONAL_DEPENDENCY("romeo_description >= 0.2")
Expand Down
2 changes: 1 addition & 1 deletion include/hpp/pinocchio/device-data.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ namespace hpp {
JACOBIAN = 0x2,
VELOCITY = 0x4,
ACCELERATION = 0x8,
COM = 0x16,
COM = 0xf,
COMPUTE_ALL = 0Xffff
};

Expand Down
18 changes: 11 additions & 7 deletions include/hpp/pinocchio/frame.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ namespace hpp {
/// Constructor
/// \param device pointer on the device the frame is belonging to.
/// \param indexInFrameList index of the frame, i.e. frame = device.model.frames[index]
Frame (DevicePtr_t device, FrameIndex indexInFrameList );
Frame (DeviceWkPtr_t device, FrameIndex indexInFrameList );

~Frame() {}
/// \}
Expand All @@ -55,11 +55,16 @@ namespace hpp {
/// Frame transformation
Transform3f currentTransformation () const;

/// Frame transformation
Transform3f currentTransformation (const DeviceData& data) const;

/// Get const reference to Jacobian
///
/// The jacobian (6d) is expressed in the local frame.
/// the linear part corresponds to the velocity of the center of the frame.
JointJacobian_t jacobian () const;
JointJacobian_t jacobian () const { return jacobian (data()); }

JointJacobian_t jacobian (const DeviceData& data) const;

///\}
// -----------------------------------------------------------------------
Expand Down Expand Up @@ -99,9 +104,9 @@ namespace hpp {
// -----------------------------------------------------------------------

/// Access robot owning the object
DeviceConstPtr_t robot () const { selfAssert(); return devicePtr_;}
DeviceConstPtr_t robot () const { selfAssert(); return devicePtr_.lock();}
/// Access robot owning the object
DevicePtr_t robot () { selfAssert(); return devicePtr_;}
DevicePtr_t robot () { selfAssert(); return devicePtr_.lock();}

/// Display frame
virtual std::ostream& display (std::ostream& os) const;
Expand All @@ -119,7 +124,7 @@ namespace hpp {
/// \}

private:
DevicePtr_t devicePtr_;
DeviceWkPtr_t devicePtr_;
FrameIndex frameIndex_;
std::vector<FrameIndex> children_;

Expand All @@ -129,8 +134,7 @@ namespace hpp {
void setChildList();
Model& model() ;
const Model& model() const ;
Data & data() ;
const Data & data() const ;
DeviceData& data() const;

/// Assert that the members of the struct are valid (no null pointer, etc).
void selfAssert() const;
Expand Down
12 changes: 6 additions & 6 deletions include/hpp/pinocchio/urdf/util.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ namespace hpp
///
/// \param robot Empty robot created before calling the function.
/// Users can pass an instance of a class deriving from Device.
/// \param baseJoint joint to which the joint tree is added.
/// \param baseFrame frame to which the joint tree is added.
/// \param prefix string to insert before all names
/// (joint, link, body names)
/// \param rootJointType type of root joint among "anchor", "freeflyer",
Expand All @@ -51,7 +51,7 @@ namespace hpp
/// \li
/// package://${package}/srdf/${modelName}${srdfSuffix}.srdf
void loadRobotModel (const DevicePtr_t& robot,
const JointIndex& baseJoint,
const FrameIndex& baseFrame,
const std::string& prefix,
const std::string& rootJointType,
const std::string& package,
Expand All @@ -74,7 +74,7 @@ namespace hpp
///
/// \param robot Empty robot created before calling the function.
/// Users can pass an instance of a class deriving from Device.
/// \param baseJoint joint to which the joint tree is added.
/// \param baseFrame frame to which the joint tree is added.
/// \param prefix string to insert before all names
/// (joint, link, body names)
/// \param rootJointType type of root joint among "anchor", "freeflyer",
Expand All @@ -86,7 +86,7 @@ namespace hpp
/// \li
/// package://${package}/urdf/${filename}.urdf
void loadUrdfModel (const DevicePtr_t& robot,
const JointIndex& baseJoint,
const FrameIndex& baseFrame,
const std::string& prefix,
const std::string& rootJointType,
const std::string& package,
Expand All @@ -101,7 +101,7 @@ namespace hpp
/// robot.
/// \param srdfPath if empty, do not try to parse SRDF.
void loadModel (const DevicePtr_t& robot,
const JointIndex& baseJoint,
const FrameIndex& baseFrame,
const std::string& prefix,
const std::string& rootType,
const std::string& urdfPath,
Expand All @@ -110,7 +110,7 @@ namespace hpp
/// Read URDF and, optionnally SRDF, as XML string.
/// \param srdfString if empty, do not try to parse SRDF.
void loadModelFromString (const DevicePtr_t& robot,
const JointIndex& baseJoint,
const FrameIndex& baseFrame,
const std::string& prefix,
const std::string& rootType,
const std::string& urdfString,
Expand Down
79 changes: 43 additions & 36 deletions src/frame.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ namespace hpp {
}
}

Frame::Frame (DevicePtr_t device, FrameIndex indexInFrameList )
Frame::Frame (DeviceWkPtr_t device, FrameIndex indexInFrameList )
:devicePtr_(device)
,frameIndex_(indexInFrameList)
{
Expand All @@ -54,18 +54,21 @@ namespace hpp {

void Frame::selfAssert() const
{
assert(devicePtr_);
assert(devicePtr_->modelPtr()); assert(devicePtr_->dataPtr());
assert(devicePtr_->model().frames.size()>std::size_t(frameIndex_));
assert(devicePtr_.lock());
assert(devicePtr_.lock()->modelPtr()); assert(devicePtr_.lock()->dataPtr());
assert(devicePtr_.lock()->model().frames.size()>std::size_t(frameIndex_));
}

inline Model& Frame::model() { selfAssert(); return devicePtr_->model(); }
inline const Model& Frame::model() const { selfAssert(); return devicePtr_->model(); }
inline Data & Frame::data() { selfAssert(); return devicePtr_->data (); }
inline const Data & Frame::data() const { selfAssert(); return devicePtr_->data (); }
inline Model& Frame::model() { selfAssert(); return devicePtr_.lock()->model(); }
inline const Model& Frame::model() const { selfAssert(); return devicePtr_.lock()->model(); }
const ::pinocchio::Frame& Frame::pinocchio() const { return model().frames[index()]; }
inline ::pinocchio::Frame& Frame::pinocchio() { return model().frames[index()]; }

DeviceData& Frame::data() const
{
return devicePtr_.lock()->d();
}

Frame Frame::parentFrame () const
{
FrameIndex idParent = model().frames[frameIndex_].previousFrame;
Expand Down Expand Up @@ -94,47 +97,51 @@ namespace hpp {
}

Transform3f Frame::currentTransformation () const
{
return currentTransformation (data());
}

Transform3f Frame::currentTransformation (const DeviceData& d) const
{
selfAssert();
const Data & d = data();
const ::pinocchio::Frame f = model().frames[frameIndex_];
if (f.type == ::pinocchio::JOINT)
return d.oMi[f.parent];
return d.data_->oMi[f.parent];
else
return d.oMi[f.parent] * f.placement;
return d.data_->oMi[f.parent] * f.placement;
}

JointJacobian_t Frame::jacobian () const
JointJacobian_t Frame::jacobian (const DeviceData& d) const
{
selfAssert();
assert(robot()->computationFlag() & JACOBIAN);
JointJacobian_t jacobian = JointJacobian_t::Zero(6,model().nv);
::pinocchio::getFrameJacobian(model(),data(),frameIndex_,::pinocchio::LOCAL,jacobian);
::pinocchio::getFrameJacobian(model(),*d.data_,frameIndex_,::pinocchio::LOCAL,jacobian);
return jacobian;
}

void Frame::setChildList()
{
assert(devicePtr_->modelPtr()); assert(devicePtr_->dataPtr());
selfAssert();
children_.clear();
if (!isFixed()) return;
const Model& model = devicePtr_->model();
const Model& m = model();

std::vector<bool> visited (model.frames.size(), false);
std::vector<bool> isChild (model.frames.size(), false);
std::vector<bool> visited (m.frames.size(), false);
std::vector<bool> isChild (m.frames.size(), false);

FrameIndex k = frameIndex_;
while (k > 0) {
visited[k] = true;
k = model.frames[k].previousFrame;
k = m.frames[k].previousFrame;
}
visited[0] = true;

for (FrameIndex i = model.frames.size() - 1; i > 0; --i) {
for (FrameIndex i = m.frames.size() - 1; i > 0; --i) {
if (visited[i]) continue;
visited[i] = true;
k = model.frames[i].previousFrame;
while (model.frames[k].type != ::pinocchio::JOINT) {
k = m.frames[i].previousFrame;
while (m.frames[k].type != ::pinocchio::JOINT) {
if (k == frameIndex_ || k == 0) break;
// if (visited[k]) {
// std::vector<FrameIndex>::iterator _k =
Expand All @@ -145,7 +152,7 @@ namespace hpp {
// break;
// }
visited[k] = true;
k = model.frames[k].previousFrame;
k = m.frames[k].previousFrame;
}
if (k == frameIndex_)
children_.push_back(i);
Expand All @@ -169,37 +176,37 @@ namespace hpp {
void Frame::positionInParentFrame (const Transform3f& p)
{
selfAssert();
devicePtr_->invalidate();
Model& model = devicePtr_->model();
GeomModel& geomModel = devicePtr_->geomModel();
devicePtr_.lock()->invalidate();
Model& m = model();
GeomModel& geomModel = devicePtr_.lock()->geomModel();
::pinocchio::Frame& me = pinocchio();
bool isJoint = (me.type == ::pinocchio::JOINT);
Transform3f fMj = (isJoint ? model.jointPlacements[me.parent].inverse() : me.placement.inverse());
Transform3f fMj = (isJoint ? m.jointPlacements[me.parent].inverse() : me.placement.inverse());
if (isJoint)
model.jointPlacements[me.parent] = model.frames[me.previousFrame].placement * p;
m.jointPlacements[me.parent] = m.frames[me.previousFrame].placement * p;
else
me.placement = model.frames[me.previousFrame].placement * p;
me.placement = m.frames[me.previousFrame].placement * p;

std::vector<bool> visited (model.frames.size(), false);
std::vector<bool> visited (m.frames.size(), false);
for (std::size_t i = 0; i < children_.size(); ++i) {
FrameIndex k = children_[i];
if (model.frames[k].type == ::pinocchio::JOINT)
k = model.frames[k].previousFrame;
if (m.frames[k].type == ::pinocchio::JOINT)
k = m.frames[k].previousFrame;
while (k != frameIndex_) {
if (visited[k]) break;
visited[k] = true;
moveFrame (model, geomModel, k, me.placement * fMj * model.frames[k].placement);
k = model.frames[k].previousFrame;
moveFrame (m, geomModel, k, me.placement * fMj * m.frames[k].placement);
k = m.frames[k].previousFrame;
}
}

// Update joint placements
for (std::size_t i = 0; i < children_.size(); ++i) {
FrameIndex k = children_[i];
const ::pinocchio::Frame f = model.frames[k];
const ::pinocchio::Frame f = m.frames[k];
if (f.type == ::pinocchio::JOINT) {
model.jointPlacements[f.parent]
= me.placement * fMj * model.jointPlacements[f.parent];
m.jointPlacements[f.parent]
= me.placement * fMj * m.jointPlacements[f.parent];
}
}
}
Expand Down
Loading