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.

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

multibody sdf: Allow specifying purely kinematic frames #9350

Closed
EricCousineau-TRI opened this issue Aug 31, 2018 · 3 comments
Closed

multibody sdf: Allow specifying purely kinematic frames #9350

EricCousineau-TRI opened this issue Aug 31, 2018 · 3 comments
Assignees

Comments

@EricCousineau-TRI
Copy link
Contributor

EricCousineau-TRI commented Aug 31, 2018

Relates #9222

At present, there is no absolutely clear way to add purely kinematic frames. Options and caveats:

Use SDFs specification for <frame/>

While very ambiguous, the number of required <frame/> tags is * per the spec:
http://sdformat.org/spec?ver=1.6&elem=model#model_frame (also defined in <link> and <joint>).
@amcastro-tri's interpretation / understanding is that this is for specifying a single frame for the object; however, after re-inspecting this, I would assume it's for adding an arbitrary number of frames.

The containers in sdformat do not currently handle <frame/> tags, e.g. Model.hh;
however, they are semi-tested in test/integration/frame.cc (only the singular case, not the plural case).

The frame would look something like:

<frame name="frame_name">
   <pose frame="parent_frame">tx ty tz rx ry rz</pose>
</frame>

While this does use <pose frame="..."> but does not enable it elsewhere --- which @hidmic's code did achieve via #6496 --- I'd personally be fine with temporary asymmetry until we can parse the frames in all SDF code.

Potential PR: #9352

Add zero-mass body.

This does not appear to be possible without hacks.
A zero-inertial body fails a positive definite matrix test, and throws about an invalid inertia (I had tried with a fixed joint welding a zero-inertial body);
I'd prefer to keep this as-is, as it keeps things physically realistic?

NOTE: I would like to strictly reject adding epsilon-mass bodies. This problem really shouldn't be that hard, and would be nice to solve correctly the first time?

Keep Drake's current abuse of <frame> tag for RBT SDF parsing:

With the following code:

void ParseSdfFrame(RigidBodyTree<double>* rigid_body_tree, XMLElement* node,
int model_instance_id) {
const char* attr = node->Attribute("drake_ignore");
if (attr && strcmp(attr, "true") == 0) return;
attr = node->Attribute("name");
if (!attr) {
throw runtime_error(string(__FILE__) + ": " + __func__ +
": ERROR: Frame tag is missing a name attribute.");
}
string name(attr);
// Parses the body.
string body_name;
if (!parseStringValue(node, "link", body_name)) {
throw runtime_error(string(__FILE__) + ": " + __func__ +
": ERROR: Frame \"" + name +
"\" doesn't have a link node.");
}
// The following will throw a std::runtime_error if the link doesn't exist.
RigidBody<double>* link =
rigid_body_tree->FindBody(body_name, "", model_instance_id);
// Get the frame's pose
XMLElement* pose = node->FirstChildElement("pose");
if (!pose) {
throw runtime_error(string(__FILE__) + ": " + __func__ +
": ERROR: Frame \"" + name +
"\" is missing its pose tag.");
}
Eigen::Vector3d rpy = Eigen::Vector3d::Zero();
Eigen::Vector3d xyz = Eigen::Vector3d::Zero();
const char* strval = pose->FirstChild()->Value();
if (strval) {
stringstream s(strval);
s >> xyz(0) >> xyz(1) >> xyz(2) >> rpy(0) >> rpy(1) >> rpy(2);
}
// Create the frame
std::shared_ptr<RigidBodyFrame<double>> frame =
allocate_shared<RigidBodyFrame<double>>(
Eigen::aligned_allocator<RigidBodyFrame<double>>(),
name, link, xyz, rpy);
rigid_body_tree->addFrame(frame);
}

RBT can parse a <frame/> from SDF as:

<frame name="frame_name">
  <link>link_name</link>
  <pose>tx ty tz rx ry rz</pose>
</frame>

We could either continue this abuse, or use XML namespaces to make something like <drake:frame/> per @jwnimmer-tri's suggestion.


Overall, I like the first option, assuming that the SDF specification of Required: * implies that we can have 0+ frames (not just 0 or 1).

@hidmic @nkoenig @scpeters If you have time, may I ask if there is an authoritative and unambiguous interpretation of <frame/> that I am missing, or if there is one that we can add?
This issue doesn't seem to pin down the definition of the <frame/> tag.

@sherm1
Copy link
Member

sherm1 commented Sep 1, 2018

FYI (not advocating this): it is actually OK to weld a massless body to a massful one -- that does not create a singular mass matrix in an internal-coordinate representation like MultibodyPlant's. For non-welds, a massless/inertialess body can cause singularities if it is the last body along a branch of a the multibody tree; I suspect our bugcatcher doesn't discriminate by joint type.

@EricCousineau-TRI
Copy link
Contributor Author

Submitted a potential solution for the first one:
#9352 - though it still feels weird that you can sprinkle <frame/> tags in other places too (which I guess we should maybe parse, and figure out what the default parent frame is in those scopes???)

@EricCousineau-TRI
Copy link
Contributor Author

EricCousineau-TRI commented Sep 5, 2018

Filed: https://bitbucket.org/osrf/sdformat/issues/200
After reviewing some of the related issues, it seems like my interpretation is correct, but would still like to briefly confirm.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

No branches or pull requests

3 participants