/// Sets the pointer to the inboard link for this joint (and updates the spatial axes, if the outboard link has been set)
void Joint::set_inboard_link(RigidBodyPtr inboard, bool update_pose)
{
_inboard_link = inboard;
if (!inboard)
return;
// add this joint to the outer joints
inboard->_outer_joints.insert(get_this());
// setup F's pose relative to the inboard
set_inboard_pose(inboard->get_pose(), update_pose);
// update articulated body pointers, if possible
if (!inboard->get_articulated_body() && !_abody.expired())
inboard->set_articulated_body(ArticulatedBodyPtr(_abody));
else if (inboard->get_articulated_body() && _abody.expired())
set_articulated_body(ArticulatedBodyPtr(inboard->get_articulated_body()));
// the articulated body pointers must now be equal; it is
// conceivable that the user is updating the art. body pointers in an
// unorthodox manner, but we'll look for this anwyway...
if (!_abody.expired())
{
ArticulatedBodyPtr abody1(inboard->get_articulated_body());
ArticulatedBodyPtr abody2(_abody);
assert(abody1 == abody2);
}
}
/**
* \note also points the outboard link to this joint
*/
void Joint::set_outboard_link(RigidBodyPtr outboard)
{
_outboard_link = outboard;
if (!outboard)
return;
//update spatial axes if both links are set
if (!_outboard_link.expired() && !_inboard_link.expired())
update_spatial_axes();
// use one articulated body pointer to set the other, if possible
if (!outboard->get_articulated_body() && !_abody.expired())
outboard->set_articulated_body(ArticulatedBodyPtr(_abody));
else if (outboard->get_articulated_body() && _abody.expired())
set_articulated_body(ArticulatedBodyPtr(outboard->get_articulated_body()));
// the articulated body pointers must now be equal; it is
// conceivable that the user is updating the art. body pointers in an
// unorthodox manner, but we'll look for this anwyway...
if (!_abody.expired())
{
ArticulatedBodyPtr abody1(outboard->get_articulated_body());
ArticulatedBodyPtr abody2(_abody);
assert(abody1 == abody2);
}
}
/**
* \note also points the outboard link to this joint
*/
void Joint::set_outboard_link(RigidBodyPtr outboard, bool update_pose)
{
_outboard_link = outboard;
if (!outboard)
return;
// add this joint to the outer joints
outboard->_inner_joints.insert(get_this());
// get the outboard pose
if (outboard->_F->rpose)
throw std::runtime_error("Joint::set_inboard_link() - relative pose on inboard link already set");
// setup Fb's pose relative to the outboard
set_outboard_pose(outboard->_F, update_pose);
// setup the frame
outboard->_xdj.pose = get_pose();
outboard->_xddj.pose = get_pose();
outboard->_Jj.pose = get_pose();
outboard->_forcej.pose = get_pose();
// use one articulated body pointer to set the other, if possible
if (!outboard->get_articulated_body() && !_abody.expired())
outboard->set_articulated_body(ArticulatedBodyPtr(_abody));
else if (outboard->get_articulated_body() && _abody.expired())
set_articulated_body(ArticulatedBodyPtr(outboard->get_articulated_body()));
// the articulated body pointers must now be equal; it is
// conceivable that the user is updating the art. body pointers in an
// unorthodox manner, but we'll look for this anwyway...
if (!_abody.expired())
{
ArticulatedBodyPtr abody1(outboard->get_articulated_body());
ArticulatedBodyPtr abody2(_abody);
assert(abody1 == abody2);
}
}
