void VigirRobotRBDLModel::updateKinematics()
{
    UpdateKinematicsCustom (my_rbdl_->rbdl_model_, &(my_rbdl_->Q_), &(my_rbdl_->QDot_), NULL);
    b_positions_up_to_date_  = true;
    b_kinematics_up_to_date_ = true;
}
 void unigripper_motoman_plant_t::propagate(const double simulation_step)
 {
     integrator->integrate(derivative_function, simulation_step);
     UpdateKinematicsCustom(model, &Q, NULL, NULL);
 }