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"); } }