void RigidBody::operator()(const RigidBody::InternalState &x, RigidBody::InternalState &dxdt, const double /* t */)
{
Eigen::Vector3d x_dot, v_dot, omega_dot;
Eigen::Vector4d q_dot;
Eigen::Vector4d q(attitude.x(), attitude.y(), attitude.z(), attitude.w());
Eigen::Vector3d omega = angularVelocity;
Eigen::Matrix4d omegaMat = Omega(omega);
x_dot = velocity;
v_dot = force/mass;
q_dot = 0.5*omegaMat*q;
omega_dot = inertia.inverse() *
(torque - omega.cross(inertia*omega));
dxdt[0] = x_dot(0);
dxdt[1] = x_dot(1);
dxdt[2] = x_dot(2);
dxdt[3] = v_dot(0);
dxdt[4] = v_dot(1);
dxdt[5] = v_dot(2);
dxdt[6] = q_dot(0);
dxdt[7] = q_dot(1);
dxdt[8] = q_dot(2);
dxdt[9] = q_dot(3);
dxdt[10] = omega_dot(0);
dxdt[11] = omega_dot(1);
dxdt[12] = omega_dot(2);
}
static void rk_osc_func(int samples, void *ptr, t_tlsmp *input){
t_rk_mother *x = ptr;
t_tlsmp data[4];
int s = samples * x->h * x->up;
int i, j;
for(i=0; i<s; i++)
{
mass1 = mass1_ctl->ctl_sig->s_block[i];
mass2 = mass2_ctl->ctl_sig->s_block[i];
sc1 = sc1_ctl->ctl_sig->s_block[i];
sc2 = sc2_ctl->ctl_sig->s_block[i];
sc3 = sc3_ctl->ctl_sig->s_block[i];
for(j=0; j<4; j++)
{
data[0] = rk_child_stage(j, x->rk_children[0]);
data[1] = rk_child_stage(j, x->rk_children[1]);
data[2] = rk_child_stage(j, x->rk_children[2]);
data[3] = rk_child_stage(j, x->rk_children[3]);
x->rk_children[0]->ks[j+1] =
y_dot(data, sc1, sc2);
x->rk_children[1]->ks[j+1] =
x_dot(data, mass1);
x->rk_children[2]->ks[j+1] =
y_dot(data, sc2, sc3);
x->rk_children[3]->ks[j+1] =
x_dot(data, mass2);
}
rk_child_estimate(x->rk_children[0]);
rk_child_estimate(x->rk_children[1]);
rk_child_estimate(x->rk_children[2]);
rk_child_estimate(x->rk_children[3]);
x->o_sigs[0]->s_block[i] = x->rk_children[0]->state;
x->o_sigs[1]->s_block[i] = x->rk_children[1]->state;
x->o_sigs[2]->s_block[i] = x->rk_children[0]->state;
x->o_sigs[3]->s_block[i] = x->rk_children[1]->state;
}
}
static void sie_osc_func2(int samples, void *ptr, t_tlsmp *input){
//this sechme requires the user to setup the functions
t_sie_mother *x = ptr;
t_tlsmp data[2];
t_tlsmp ret[2];
int s = samples * x->h * x->up;
int i, j;
int its = 100;
for(i=0; i<s; i++)
{
freq = 2 * M_PI * ctl_freq->ctl_sig->s_block[i];
/*x*/
data[0] = x->sie_children[0]->state;
/*y*/
data[1] = x->sie_children[1]->state;
x->sie_children[0]->state = x->sie_children[0]->state + x->h * x_dot(data, freq);
x->sie_children[1]->state = x->sie_children[1]->state + x->h * y_dot(data, freq);
x->o_sigs[0]->s_block[i] = x->sie_children[0]->state;
x->o_sigs[1]->s_block[i] = x->sie_children[1]->state;
}
}
// NOTE: Odometry data is used to set the 6 virtual degrees of freedom.
void OdometryStateReceiverSM::odometryMessageCallback(nav_msgs::Odometry & odom)
{
// CONTROLIT_INFO_RT << "Method called!";
if (!rcvdInitOdomMsg)
{
CONTROLIT_INFO_RT << "Received initial odometry message.";
rcvdInitOdomMsg = true;
}
else
{
// if (!rcvdFirstOdomMsg)
// {
// CONTROLIT_INFO_RT << "Received first odometry message.";
// rcvdFirstOdomMsg = true;
// }
// Assuming odometry information in the correct frame...
Vector x(3);
x(0) = odom.pose.pose.position.x;
x(1) = odom.pose.pose.position.y;
x(2) = odom.pose.pose.position.z;
// Get the orientation
Eigen::Quaterniond q(odom.pose.pose.orientation.w,
odom.pose.pose.orientation.x,
odom.pose.pose.orientation.y,
odom.pose.pose.orientation.z);
// Get the velocity
Vector x_dot(6);
x_dot(0) = odom.twist.twist.linear.x;
x_dot(1) = odom.twist.twist.linear.y;
x_dot(2) = odom.twist.twist.linear.z;
x_dot(3) = odom.twist.twist.angular.x;
x_dot(4) = odom.twist.twist.angular.y;
x_dot(5) = odom.twist.twist.angular.z;
if (!controlit::addons::eigen::checkMagnitude(x) || !controlit::addons::eigen::checkMagnitude(q) || !controlit::addons::eigen::checkMagnitude(x_dot))
{
CONTROLIT_ERROR_RT << "Received invalid odometry data!\n"
<< " - x: " << x.transpose() << "\n"
<< " - q: [" << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z() << "]\n"
<< " - x_dot: " << x_dot.transpose();
assert(false);
}
else
{
// the writeFromNonRT can be used in RT, if you have the guarantee that
// * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
// * there is only one single rt thread
odometryDataBuffer.writeFromNonRT( OdometryData(x, q, x_dot) );
receivedOdometryState = true;
}
}
}
static void ie_osc_func(int samples, void *ptr, t_tlsmp *input){
t_ie_mother *x = ptr;
t_tlsmp data[2];
t_tlsmp ret[2];
int s = samples * x->h * x->up;
int i, j;
int its = 100;
for(i=0; i<s; i++)
{
freq = 2 * M_PI * ctl_freq->ctl_sig->s_block[i];
ie_child_begin(x->ie_children[0]);
ie_child_begin(x->ie_children[1]);
//printf("0 diff %f 0 tol %f 1 diff %f 1 tol %f\n", x->ie_children[0]->diff, x->ie_children[0]->tol, x->ie_children[0]->diff, x->ie_children[0]->tol);
while(its--)
{
data[0] = x->ie_children[0]->y_k01;
data[1] = x->ie_children[1]->y_k01;
x->ie_children[0]->y_k11 = x->ie_children[0]->y_k + x->h_time *
x_dot(data, freq);
ie_child_iterate(x->ie_children[0]);
x->ie_children[1]->y_k11 = x->ie_children[1]->y_k + x->h_time *
y_dot(data, freq);
ie_child_iterate(x->ie_children[1]);
if(x->ie_children[0]->diff<=x->ie_children[0]->tol &&
x->ie_children[1]->diff<=x->ie_children[1]->tol) break;
}
x->o_sigs[0]->s_block[i] = x->ie_children[0]->state;
x->o_sigs[1]->s_block[i] = x->ie_children[1]->state;
}
}
static void rk_osc_func(int samples, void *ptr, t_tlsmp *input){
t_rk_mother *x = ptr;
t_tlsmp data[2];
t_tlsmp ret[2];
int s = samples * x->h * x->up;
int i, j;
for(i=0; i<s; i++)
{
freq = 2 * M_PI * ctl_freq->ctl_sig->s_block[i];
for(j=0; j<4; j++)
{
//printf("%f %f %f\n", data1[0], fb1,
/*x0*/
data[0] = rk_child_stage(j, x->rk_children[0]);
/*y0*/
data[1] = rk_child_stage(j, x->rk_children[1]);
x->rk_children[0]->ks[j+1] =
x_dot(data, freq);
x->rk_children[1]->ks[j+1] =
y_dot(data, freq);
}
rk_child_estimate(x->rk_children[0]);
rk_child_estimate(x->rk_children[1]);
x->o_sigs[0]->s_block[i] = x->rk_children[0]->state;
x->o_sigs[1]->s_block[i] = x->rk_children[1]->state;
}
}
static void cd_osc_func(int samples, void *ptr, t_tlsmp *input){
t_cd_mother *x = ptr;
t_tlsmp data[2];
int s = samples * x->h * x->up;
int i, j;
for(i=0; i<s; i++)
{
freq = 2 * M_PI * ctl_freq->ctl_sig->s_block[i];
//printf("%f %f %f\n", data1[0], fb1,
/*x0*/
data[0] = cd_child_stage(0, x->cd_children[0]);
/*y0*/
data[1] = cd_child_stage(1, x->cd_children[1]);
//fprintf(fp, "data: %f %f\n", data[0], data[1]);
x->cd_children[0]->sides[0] =
x_dot(data, freq);
x->cd_children[1]->sides[1] =
y_dot(data, freq);
//fprintf(fp, "sides: %f %f\n", x->cd_children[0]->sides[j], x->cd_children[1]->sides[j]);
cd_child_estimate(x->cd_children[0]);
cd_child_estimate(x->cd_children[1]);
x->o_sigs[0]->s_block[i] = x->cd_children[0]->state;
x->o_sigs[1]->s_block[i] = x->cd_children[1]->state;
}
}
static void rk_osc_func(int samples, void *ptr, t_sample *input){
t_rk_mother *x = ptr;
int s = samples * x->h * x->up;
int i, j;
t_sample data0[2], data1[2];
t_sample old_states[2], cur_states[2];
for(i=0; i< s; i++)
{
sqr_flyback0 = ctls[0]->c_sig[i];
sqr_flyback1 = ctls[1]->c_sig[i];
sqr_curl0 = ctls[2]->c_sig[i];
sqr_curl1 = ctls[3]->c_sig[i];
sqr_gain0 = ctls[4]->c_sig[i];
sqr_gain1 = ctls[5]->c_sig[i];
tri_curl0 = ctls[6]->c_sig[i];
tri_curl1 = ctls[7]->c_sig[i];
tri_thresh0 = ctls[8]->c_sig[i];
tri_thresh1 = ctls[9]->c_sig[i];
for(j=0; j<4; j++)
{
//printf("%f %f %f\n", data1[0], fb1,
/*x0*/
data0[0] = rk_child_stage(j, x->rk_children[0]);
/*y0*/
data0[1] = rk_child_stage(j, x->rk_children[1]);
/*x1*/
data1[0] = rk_child_stage(j, x->rk_children[2]);
/*y1*/
data1[1] = rk_child_stage(j, x->rk_children[3]);
x->rk_children[0]->ks[j+1] =
x_dot(data0, sqr_gain0, tri_curl0);
x->rk_children[1]->ks[j+1] =
y_dot(data0, tri_thresh0, sqr_flyback0);
x->rk_children[2]->ks[j+1] =
x_dot(data1, sqr_gain1, tri_curl1);
x->rk_children[3]->ks[j+1] =
y_dot(data1, tri_thresh1, sqr_flyback1);
}
rk_child_estimate(x->rk_children[0]);
rk_child_estimate(x->rk_children[1]);
rk_child_estimate(x->rk_children[2]);
rk_child_estimate(x->rk_children[3]);
cur_states[0] = x->rk_children[0]->state;
cur_states[1] = x->rk_children[2]->state;
check_sync_matrix(old_states, cur_states);
old_states[0] = x->o_sigs[0]->s_block[i] =
x->rk_children[0]->state;
x->o_sigs[1]->s_block[i] =
tanh_clip(x->rk_children[1]->state, sqr_curl0);
old_states[1] = x->o_sigs[2]->s_block[i] =
x->rk_children[2]->state;
x->o_sigs[3]->s_block[i] =
tanh_clip(x->rk_children[3]->state, sqr_curl1);
}
}
static void rk_osc_func(int samples, void *ptr, t_tlsmp *input){
t_rk_mother *x = ptr;
int s = samples * x->h * x->up;
int i, j, k;
t_tlsmp d2us[no_mass], ys[no_mass];
for(i=0; i<s; i++)
{
for(j=0; j<4; j++)
{
//d2us
for(k=0; k<no_mass; k++)
{
//printf("k : %d\n", k);
ys[k] = rk_child_stage(j, x->rk_children[k]);
}
//ys
for(k=no_mass; k<no_mass*2; k++)
{
//printf("k : %d, k-no_mass : %d\n", k, k-no_mass);
d2us[k-no_mass] = rk_child_stage(j, x->rk_children[k]);
}
//spring constants
for(k=0; k<no_mass+1; k++)
ks[k] = sc_ctls[k]->ctl_sig->s_block[i];
//masses
for(k=0; k<no_mass; k++)
ms[k] = mass_ctls[k]->ctl_sig->s_block[i];
//d2us first
//boundaries
x->rk_children[0]->ks[j+1] =
y_dot(0, d2us[0], d2us[1], 1000, ks[0]*.5, ks[1]*.5, ks[2] *.5, ms[0]);
x->rk_children[no_mass-1]->ks[j+1] =
y_dot(d2us[no_mass-2], d2us[no_mass-1], 0, ks[no_mass - 2]*.5, ks[no_mass-1]*.5, ks[no_mass],1000, ms[no_mass-1]);//-1 to count from 0
//printf();
//inner d2us
for(k=1; k<no_mass-1; k++)
{
// printf("\tj : %d, k : %d\n", j, k);
x->rk_children[k]->ks[j+1] =
y_dot(d2us[k-1], d2us[k], d2us[k+1], ks[k-1]*.5, ks[k]*.5, ks[k+1]*.5, ks[k+2] *.5, ms[k]);
}
//ys
for(k=no_mass; k<no_mass*2; k++)
{
//printf("k(ys):%d\n", k);
x->rk_children[k]->ks[j+1] =
x_dot(ys[k-no_mass]);
}
}
//printf("%d\n", i);
for(k=0; k<no_mass*2; k++)
{
rk_child_estimate(x->rk_children[k]);
x->o_sigs[k]->s_block[i] = x->rk_children[k]->state;
/* if(k>=no_mass) */
/* printf("state : %d val : %1.5f ", k, x->rk_children[k]->state); */
}
// printf("\n");
}
}
