int
main(int argc, char** argv)
{
if (argc < 2)
{
std::cout << "Usage: rlJacobianDemo KINEMATICSFILE Q1 ... Qn QD1 ... QDn" << std::endl;
return 1;
}
try
{
boost::shared_ptr< rl::kin::Kinematics > kinematics(rl::kin::Kinematics::create(argv[1]));
rl::math::Vector q(kinematics->getDof());
for (std::ptrdiff_t i = 0; i < q.size(); ++i)
{
q(i) = boost::lexical_cast< rl::math::Real >(argv[i + 2]);
}
rl::math::Vector qdot(kinematics->getDof());
for (std::ptrdiff_t i = 0; i < qdot.size(); ++i)
{
qdot(i) = boost::lexical_cast< rl::math::Real >(argv[q.size() + i + 2]);
}
kinematics->setPosition(q);
kinematics->updateFrames();
kinematics->updateJacobian();
std::cout << "J=" << std::endl << kinematics->getJacobian() << std::endl;
rl::math::Vector xdot(kinematics->getOperationalDof() * 6);
kinematics->forwardVelocity(qdot, xdot);
std::cout << "xdot=" << std::endl;
for (std::size_t i = 0; i < kinematics->getOperationalDof(); ++i)
{
std::cout << i << " " << xdot.transpose() << std::endl;
}
kinematics->updateJacobianInverse(1.0e-9f);
std::cout << "invJ=" << std::endl << kinematics->getJacobianInverse() << std::endl;
kinematics->inverseVelocity(xdot, qdot);
std::cout << "qdot=" << std::endl << qdot.transpose() << std::endl;
}
catch (const std::exception& e)
{
std::cout << e.what() << std::endl;
return 1;
}
return 0;
}
void Estimator::run()
{
float acc_kp, ki;
uint64_t now_us = RF_.sensors_.data().imu_time;
if (last_time_ == 0)
{
last_time_ = now_us;
last_acc_update_us_ = last_time_;
return;
}
else if (now_us < last_time_)
{
// this shouldn't happen
RF_.state_manager_.set_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
last_time_ = now_us;
return;
}
RF_.state_manager_.clear_error(StateManager::ERROR_TIME_GOING_BACKWARDS);
float dt = (now_us - last_time_) * 1e-6f;
last_time_ = now_us;
state_.timestamp_us = now_us;
// Crank up the gains for the first few seconds for quick convergence
if (now_us < static_cast<uint64_t>(RF_.params_.get_param_int(PARAM_INIT_TIME))*1000)
{
acc_kp = RF_.params_.get_param_float(PARAM_FILTER_KP)*10.0f;
ki = RF_.params_.get_param_float(PARAM_FILTER_KI)*10.0f;
}
else
{
acc_kp = RF_.params_.get_param_float(PARAM_FILTER_KP);
ki = RF_.params_.get_param_float(PARAM_FILTER_KI);
}
// Run LPF to reject a lot of noise
run_LPF();
// add in accelerometer
float a_sqrd_norm = accel_LPF_.sqrd_norm();
turbomath::Vector w_acc;
if (RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)
&& a_sqrd_norm < 1.1f*1.1f*9.80665f*9.80665f && a_sqrd_norm > 0.9f*0.9f*9.80665f*9.80665f)
{
// Get error estimated by accelerometer measurement
last_acc_update_us_ = now_us;
// turn measurement into a unit vector
turbomath::Vector a = accel_LPF_.normalized();
// Get the quaternion from accelerometer (low-frequency measure q)
// (Not in either paper)
turbomath::Quaternion q_acc_inv(g_, a);
// Get the error quaternion between observer and low-freq q
// Below Eq. 45 Mahony Paper
turbomath::Quaternion q_tilde = q_acc_inv * state_.attitude;
// Correction Term of Eq. 47a and 47b Mahony Paper
// w_acc = 2*s_tilde*v_tilde
w_acc.x = -2.0f*q_tilde.w*q_tilde.x;
w_acc.y = -2.0f*q_tilde.w*q_tilde.y;
w_acc.z = 0.0f; // Don't correct z, because it's unobservable from the accelerometer
// integrate biases from accelerometer feedback
// (eq 47b Mahony Paper, using correction term w_acc found above
bias_.x -= ki*w_acc.x*dt;
bias_.y -= ki*w_acc.y*dt;
bias_.z = 0.0; // Don't integrate z bias, because it's unobservable
}
else
{
w_acc.x = 0.0f;
w_acc.y = 0.0f;
w_acc.z = 0.0f;
}
if (attitude_correction_next_run_)
{
attitude_correction_next_run_ = false;
w_acc += RF_.params_.get_param_float(PARAM_FILTER_KP_ATT_CORRECTION)*(q_correction_ - state_.attitude);
}
// Handle Gyro Measurements
turbomath::Vector wbar;
if (RF_.params_.get_param_int(PARAM_FILTER_USE_QUAD_INT))
{
// Quadratic Interpolation (Eq. 14 Casey Paper)
// this step adds 12 us on the STM32F10x chips
wbar = (w2_/-12.0f) + w1_*(8.0f/12.0f) + gyro_LPF_ * (5.0f/12.0f);
w2_ = w1_;
w1_ = gyro_LPF_;
}
else
{
wbar = gyro_LPF_;
}
// Build the composite omega vector for kinematic propagation
// This the stuff inside the p function in eq. 47a - Mahony Paper
turbomath::Vector wfinal = wbar - bias_ + w_acc * acc_kp;
// Propagate Dynamics (only if we've moved)
float sqrd_norm_w = wfinal.sqrd_norm();
if (sqrd_norm_w > 0.0f)
{
float p = wfinal.x;
float q = wfinal.y;
float r = wfinal.z;
if (RF_.params_.get_param_int(PARAM_FILTER_USE_MAT_EXP))
{
// Matrix Exponential Approximation (From Attitude Representation and Kinematic
// Propagation for Low-Cost UAVs by Robert T. Casey)
// (Eq. 12 Casey Paper)
// This adds 90 us on STM32F10x chips
float norm_w = sqrtf(sqrd_norm_w);
turbomath::Quaternion qhat_np1;
float t1 = cosf((norm_w*dt)/2.0f);
float t2 = 1.0f/norm_w * sinf((norm_w*dt)/2.0f);
qhat_np1.w = t1*state_.attitude.w + t2*(-p*state_.attitude.x - q*state_.attitude.y - r*state_.attitude.z);
qhat_np1.x = t1*state_.attitude.x + t2*( p*state_.attitude.w + r*state_.attitude.y - q*state_.attitude.z);
qhat_np1.y = t1*state_.attitude.y + t2*( q*state_.attitude.w - r*state_.attitude.x + p*state_.attitude.z);
qhat_np1.z = t1*state_.attitude.z + t2*( r*state_.attitude.w + q*state_.attitude.x - p*state_.attitude.y);
state_.attitude = qhat_np1.normalize();
}
else
{
// Euler Integration
// (Eq. 47a Mahony Paper), but this is pretty straight-forward
turbomath::Quaternion qdot(0.5f * (-p*state_.attitude.x - q*state_.attitude.y - r*state_.attitude.z),
0.5f * ( p*state_.attitude.w + r*state_.attitude.y - q*state_.attitude.z),
0.5f * ( q*state_.attitude.w - r*state_.attitude.x + p*state_.attitude.z),
0.5f * ( r*state_.attitude.w + q*state_.attitude.x - p*state_.attitude.y));
state_.attitude.w += qdot.w*dt;
state_.attitude.x += qdot.x*dt;
state_.attitude.y += qdot.y*dt;
state_.attitude.z += qdot.z*dt;
state_.attitude.normalize();
}
}
// Extract Euler Angles for controller
state_.attitude.get_RPY(&state_.roll, &state_.pitch, &state_.yaw);
// Save off adjust gyro measurements with estimated biases for control
state_.angular_velocity = gyro_LPF_ - bias_;
// If it has been more than 0.5 seconds since the acc update ran and we are supposed to be getting them
// then trigger an unhealthy estimator error
if (RF_.params_.get_param_int(PARAM_FILTER_USE_ACC) && now_us > 500000 + last_acc_update_us_
&& !RF_.params_.get_param_int(PARAM_FIXED_WING))
{
RF_.state_manager_.set_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR);
}
else
{
RF_.state_manager_.clear_error(StateManager::ERROR_UNHEALTHY_ESTIMATOR);
}
}
void InverseDynamicsExample::stepSimulation(float deltaTime)
{
if(m_multiBody) {
const int num_dofs=m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs), qdot(num_dofs), q(num_dofs),joint_force(num_dofs);
btInverseDynamics::vecx pd_control(num_dofs);
// compute joint forces from one of two control laws:
// 1) "computed torque" control, which gives perfect, decoupled,
// linear second order error dynamics per dof in case of a
// perfect model and (and negligible time discretization effects)
// 2) decoupled PD control per joint, without a model
for(int dof=0;dof<num_dofs;dof++) {
q(dof) = m_multiBody->getJointPos(dof);
qdot(dof)= m_multiBody->getJointVel(dof);
const btScalar qd_dot=0;
const btScalar qd_ddot=0;
if (m_timeSeriesCanvas)
m_timeSeriesCanvas->insertDataAtCurrentTime(q[dof],dof,true);
// pd_control is either desired joint torque for pd control,
// or the feedback contribution to nu
pd_control(dof) = kd*(qd_dot-qdot(dof)) + kp*(qd[dof]-q(dof));
// nu is the desired joint acceleration for computed torque control
nu(dof) = qd_ddot + pd_control(dof);
}
if(useInverseModel)
{
// calculate joint forces corresponding to desired accelerations nu
if (m_multiBody->hasFixedBase())
{
if(-1 != m_inverseModel->calculateInverseDynamics(q,qdot,nu,&joint_force))
{
//joint_force(dof) += damping*dot_q(dof);
// use inverse model: apply joint force corresponding to
// desired acceleration nu
for(int dof=0;dof<num_dofs;dof++)
{
m_multiBody->addJointTorque(dof,joint_force(dof));
}
}
} else
{
//the inverse dynamics model represents the 6 DOFs of the base, unlike btMultiBody.
//append some dummy values to represent the 6 DOFs of the base
btInverseDynamics::vecx nu6(num_dofs+6), qdot6(num_dofs+6), q6(num_dofs+6),joint_force6(num_dofs+6);
for (int i=0;i<num_dofs;i++)
{
nu6[6+i] = nu[i];
qdot6[6+i] = qdot[i];
q6[6+i] = q[i];
joint_force6[6+i] = joint_force[i];
}
if(-1 != m_inverseModel->calculateInverseDynamics(q6,qdot6,nu6,&joint_force6))
{
//joint_force(dof) += damping*dot_q(dof);
// use inverse model: apply joint force corresponding to
// desired acceleration nu
for(int dof=0;dof<num_dofs;dof++)
{
m_multiBody->addJointTorque(dof,joint_force6(dof+6));
}
}
}
} else
{
for(int dof=0;dof<num_dofs;dof++)
{
// no model: just apply PD control law
m_multiBody->addJointTorque(dof,pd_control(dof));
}
}
}
if (m_timeSeriesCanvas)
m_timeSeriesCanvas->nextTick();
//todo: joint damping for btMultiBody, tune parameters
// step the simulation
if (m_dynamicsWorld)
{
// todo(thomas) check that this is correct:
// want to advance by 10ms, with 1ms timesteps
m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3);
btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m;
m_multiBody->forwardKinematics(scratch_q, scratch_m);
#if 0
for (int i = 0; i < m_multiBody->getNumLinks(); i++)
{
//btVector3 pos = m_multiBody->getLink(i).m_cachedWorldTransform.getOrigin();
btTransform tr = m_multiBody->getLink(i).m_cachedWorldTransform;
btVector3 pos = tr.getOrigin() - quatRotate(tr.getRotation(), m_multiBody->getLink(i).m_dVector);
btVector3 localAxis = m_multiBody->getLink(i).m_axes[0].m_topVec;
//printf("link %d: %f,%f,%f, local axis:%f,%f,%f\n", i, pos.x(), pos.y(), pos.z(), localAxis.x(), localAxis.y(), localAxis.z());
}
#endif
}
}
int
main()
{
try
{
vpRobotPtu46 robot ;
vpColVector q(2) ;
vpERROR_TRACE(" ") ;
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
q = 0;
vpCTRACE << "Set position in the articular frame: " << q.t();
robot.setPosition(vpRobot::ARTICULAR_FRAME, q) ;
q[0] = vpMath::rad(10);
q[1] = vpMath::rad(20);
vpCTRACE << "Set position in the articular frame: " << q.t();
robot.setPosition(vpRobot::ARTICULAR_FRAME, q) ;
vpColVector qm(2) ;
robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
vpCTRACE << "Position in the articular frame " << qm.t() ;
vpColVector qdot(2) ;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
#if 0
qdot = 0 ;
qdot[0] = vpMath::rad(10) ;
qdot[1] = vpMath::rad(10) ;
vpCTRACE << "Set camera frame velocity " << qdot.t() ;
robot.setVelocity(vpRobot::CAMERA_FRAME, qdot) ;
sleep(2) ;
qdot = 0 ;
qdot[0] = vpMath::rad(-10) ;
qdot[1] = vpMath::rad(-10) ;
vpCTRACE << "Set camera frame velocity " << qdot.t() ;
robot.setVelocity(vpRobot::CAMERA_FRAME, qdot) ;
sleep(2) ;
#endif
qdot = 0 ;
// qdot[0] = vpMath::rad(0.1) ;
qdot[1] = vpMath::rad(10) ;
vpCTRACE << "Set articular frame velocity " << qdot.t() ;
robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot) ;
sleep(2) ;
qdot = 0 ;
qdot[0] = vpMath::rad(-5);
//qdot[1] = vpMath::rad(-5);
vpCTRACE << "Set articular frame velocity " << qdot.t() ;
robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot) ;
sleep(2) ;
}
catch (...)
{
std::cout << "Sorry PtU46 not available ..." << std::endl;
}
return 0;
}
void quat_magnitude(const Quat q, float* r) {
*r = (float)(sqrt(qdot(q, q)));
}
