/// (Relatively slow) method for determining the joint velocity from current link velocities
void Joint::determine_q_dot()
{
MatrixNd m, U, V;
VectorNd S;
// get the spatial axes
const vector<SVelocityd>& s = get_spatial_axes();
// convert to a matrix
to_matrix(s, m);
// compute the SVD
_LA.svd(m, U, S, V);
// get the velocities in computation frames
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
const SVelocityd& vi = inboard->get_velocity();
const SVelocityd& vo = outboard->get_velocity();
// get velocities in s's frame
shared_ptr<const Pose3d> spose = get_pose();
SVelocityd svi = Pose3d::transform(spose, vi);
SVelocityd svo = Pose3d::transform(spose, vo);
// compute the change in velocity
m.mult(svo - svi, this->qd);
}
/// Adds gravity to a body
void StokesDragForce::add_force(DynamicBodyPtr body)
{
// if the body is rigid, add drag
RigidBodyPtr rb = dynamic_pointer_cast<RigidBody>(body);
if (rb)
{
SForced w;
w.set_force(rb->get_velocity().get_linear() * -this->b);
w.set_torque(rb->get_velocity().get_angular() * -this->b_ang);
w.pose = rb->get_velocity().pose;
SForced wx = Pose3d::transform(rb->get_computation_frame(), w);
rb->add_force(wx);
}
else
{
// it's an articulated body, get it as such
ArticulatedBodyPtr ab = boost::dynamic_pointer_cast<ArticulatedBody>(body);
// get the vector of links
const std::vector<RigidBodyPtr>& links = ab->get_links();
// apply drag force to all links
BOOST_FOREACH(RigidBodyPtr rb, links)
{
SForced w;
w.set_force(rb->get_velocity().get_linear() * -this->b);
w.set_torque(rb->get_velocity().get_angular() * -this->b_ang);
w.pose = rb->get_velocity().pose;
SForced wx = Pose3d::transform(rb->get_computation_frame(), w);
rb->add_force(wx);
}
}
