/*****************************************************************************
******************************************************************************
Function Name : run_rosrt_test_task
Date : Dec. 1997
Remarks:
run the task from the task servo: REAL TIME requirements!
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
run_rosrt_test_task(void)
{
int j, i;
double task_time;
double omega;
int dof;
ros_run();
// NOTE: all array indices start with 1 in SL
task_time = task_servo_time - start_time;
omega = 2.0*PI*freq;
// osciallates one DOF
dof = 1;
for (i=dof; i<=dof; ++i) {
target[i].th = joint_default_state[i].th +
amp*sin(omega*task_time);
target[i].thd = amp*omega*cos(omega*task_time);
target[i].thdd =-amp*omega*omega*sin(omega*task_time);
}
// the following variables need to be assigned
for (i=1; i<=N_DOFS; ++i) {
joint_des_state[i].th = target[i].th;
joint_des_state[i].thd = target[i].thd;
joint_des_state[i].thdd = target[i].thdd;
joint_des_state[i].uff = 0.0;
}
// compute inverse dynamics torques
SL_InvDynNE(joint_state,joint_des_state,endeff,&base_state,&base_orient);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : run_elbow_perturbation
Date : January 2015
Remarks:
run the task from the task servo: REAL TIME requirements!
******************************************************************************
Parameters: (i/o = input/output)
none
*****************************************************************************/
static int
run_elbow_perturbation(void)
{
int i;
double tau;
double task_time;
task_time = task_servo_time - start_time;
// some extra filtering for better inertial compensation (inherited from Michael Mistry's program)
for (i=1; i<=N_DOFS; ++i)
{
joint_filt_state[i] = extra_joint_state_filter(i);
}
if (joint_state[R_TL].th < R_TL_threshold) // if joint R_TL is below threshold (thumb-in switch is activated)
{
for (i=1; i<=N_DOFS; ++i)
{
// The one that is very noisy is the joint acceleration sensing, that's why we are using the filtered version of it
// (joint_filt_state[i].thdd), instead of the unfiltered one (joint_state[i].thdd). Joint position (joint_state[i].th) and joint velocity (joint_state[i].thd)
// on the other hand are not so noisy, so we are using the original (unfiltered) one.
if (i == R_EB)
{
joint_des_state[i].th = joint_state[i].th;
joint_des_state[i].thd = joint_state[i].thd;
joint_des_state[i].thdd = joint_filt_state[i].thdd * inertia_gain; // multiplication by inertia gain here is maybe for reducing the risk of the system becoming unstable???
joint_des_state[i].uff = 0.0;
}
else
{
joint_des_state[i].th = joint_des_state[i].th; // don't change the joint_des_state;
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
joint_des_state[i].uff = 0.0;
}
}
SL_InvDynNE(joint_state,joint_des_state,endeff,&base_state,&base_orient);
for (i=1; i<=N_DOFS; ++i)
{
if (i == R_EB)
{
// Command joint PD servo to track the current state, in other words, cancel out the joint feedback control:
joint_des_state[i].th = joint_state[i].th;
joint_des_state[i].thd = joint_state[i].thd;
joint_des_state[i].thdd = joint_state[i].thdd;
if (i < R_TL)
{
tau = kp_drag_gain * (joint_des_drag_state[i].th-joint_state[i].th);
//tau = kp_drag_gain * (joint_des_drag_state[i].th-joint_state[i].th) +
// exp(-kd_damping_factor*fabs(joint_state[i].thd)) * kd_drag_gain * (-joint_state[i].thd);
if (fabs(tau) > f_saturation)
{
tau = macro_sign(tau) * f_saturation;
}
joint_des_state[i].uff += tau;
if (fabs(tau) > drag_threshold)
{
joint_des_drag_state[i].th = joint_state[i].th;
}
}
}
}
}
else // if (joint_state[R_TL].th >= R_TL_threshold), or in other words if (thumb-in switch is NOT activated)
{
// inverse dynamics for dynamic compensation
for (i=1; i<=N_DOFS; ++i)
{
joint_des_state[i].th = joint_des_state[i].th; // don't change the joint_des_state;
joint_des_state[i].thd = 0.0;
joint_des_state[i].thdd = 0.0;
joint_des_state[i].uff = 0.0;
}
SL_InvDynNE(joint_state,joint_des_state,endeff,&base_state,&base_orient);
}
}
/*
* Execute trajectory with IDM and save the corresponding feedforward commands to traj_uff
*/
static void track_with_idm(int real_flag) {
int i,j;
static int fb_flag = FALSE;
printf("Initializing feedforward commands...\n");
//printf("Toggling nominal inverse dynamics...\n");
//read_link_parameters("LinkParametersNom.cf");
if (!real_flag) {
SL_DJstate testq[N_DOFS+1];
bzero((void *)testq, sizeof(SL_DJstate)* (N_DOFS+1));
for (j = 1; j <= traj_len-1; j++) { // traj index j
for (i = 1; i <= N_DOFS; i++) {
testq[i].th = traj_pos[j][i];
testq[i].thd = traj_vel[j][i];
testq[i].thdd = traj_acc[j][i];
}
SL_InvDynNE(NULL,testq,endeff,&base_state,&base_orient);
if (friction_comp) {
addFrictionModel(testq);
}
// save uff received
for(i = 1; i <= N_DOFS; i++) {
traj_uff[j][i] = testq[i].uff;
}
}
}
else {
for (j = 1; j <= traj_len-1; j++) { // traj index j
for (i = 1; i <= N_DOFS; i++) {
joint_des_state[i].th = traj_pos[j][i];
joint_des_state[i].thd = traj_vel[j][i];
joint_des_state[i].thdd = traj_acc[j][i];
}
if (fb_flag) {
SL_InvDynArt(joint_state,joint_des_state,endeff,&base_state,&base_orient);
}
else {
SL_InvDynArt(NULL,joint_des_state,endeff,&base_state,&base_orient);
}
if (friction_comp) {
addFrictionModel(joint_des_state);
}
// save calculated feedforward command
save_uff(j);
}
save_uff(traj_len);
}
//printf("Toggling back the optimized inverse dynamics parameters for returning traj...\n");
//read_link_parameters(config_files[LINKPARAMETERS]);
return;
}
/*****************************************************************************
******************************************************************************
Function Name : run_turing_playback_task
Date : June 2014
Remarks:
run the task from the task servo: REAL TIME requirements!
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
run_turing_playback_task(void)
{
static int firsttime = TRUE;
int j, i, dof;
double task_time;
double wait_time = 5;
//double trigg_start, trigg_dur;
//double start_elbow_angle;
double def_elbow_angle = 1.571; //1.571rad = 90deg
double shift = 0.2; //time shift for position
double perturbAmp = 0.3491; //0.3491rad = 20deg | 0.5236 = 30deg
double b = 43.8; // 31.9 for perturbAmp = 30deg;
/////////////////////////////////////////////////////////////////////////////////////////////
// RANDOMIZATION
//srand(time(NULL)); //initialize random number generator
// for random perturbation: perturb_satart_time = rand() %10 + 5;
/////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////
// trigger parameters
//trigg_start = 1.0; //trigger start time (s)
//trigg_dur = 0.5; // duration of the pulse (s)
/////////////////////////////////////////////////////////////////////////////////////////////
// Task parameters
task_time = task_servo_time - start_time;
/*// Tirgger event for starting EMG recording ///////////////////////////////////////////////
if (task_time >= trigg_start && dataFlag == 0)
{
setOsc(d2a_cr,75.00);
emgTrig = 5.0;
dataFlag = 1;
printf("Data collection started\n");
scd(); // start data collection; manually input stopcd and then saveData from console
}
else
{
if (task_time >= trigg_start+trigg_dur)
{
setOsc(d2a_cr,50.00);
emgTrig = 0.0;
}
}*/////////////////////////////////////////////////////////////////////////////////////////
// osciallates one DOF
//dof = 4; // degree of freedom to rotate
//for (i=dof; i<=dof; ++i)
//{
/*
//move to desired_pos_f
if(joint_state[i].th <= def_elbow_angle && udFlag == 0)
{
this_time = task_time - last_time;
some_time = task_time;
//direction = 0;
if (this_time >= wait_time)
{
direction = 1;
start_elbow_angle = def_elbow_angle;
setOsc(d2a_cr,75.00);
emgTrig = 5.0;
udFlag = 1;
last_time = task_time;
}
}
//move to desired_pos_i
if(joint_state[i].th >= def_elbow_angle+perturbAmp && udFlag == 1)
{
setOsc(d2a_cr,50.00);
emgTrig = 0.0;
this_time = task_time - last_time;
some_time = task_time;
//direction = 0;
if (this_time >= wait_time)
{
direction = -1;
start_elbow_angle = def_elbow_angle + perturbAmp;
last_time = task_time;
udFlag = 0;
}
}
mov_time = task_time - some_time;
target[i].th = start_elbow_angle + direction*perturbAmp/(1 + exp(-b*(mov_time-shift)));
target[i].thd = direction*perturbAmp*b*exp(-b*(mov_time-shift))/pow(1+exp(-b*(mov_time-shift)), 2);
target[i].thdd = direction*perturbAmp*(pow(b,2)*exp(-b*(mov_time-shift))/pow(1+exp(-b*(mov_time-shift)),2))*(2*exp(-b*(mov_time-shift))/(1+exp(-b*(mov_time-shift)))-1);
*/
if (ivar == 1)
{
if (firsttime == TRUE)
{
direction = -1;
firsttime = FALSE;
}
/*
if (this_time >= wait_time)
{
direction = -direction;
start_elbow_angle = joint_state[i].th;
last_time = task_time;
some_time = task_time;
if (direction > 0) //if it's returning to defaul position
{
setOsc(d2a_cr,75.00);
//emgTrig = 5.0;
}
else
{
setOsc(d2a_cr,50.00);
//emgTrig = 0.0;
ivar = 0;
}
}
*/
}
this_time = task_time - last_time;
mov_time = task_time - some_time;
/*
target[i].th = start_elbow_angle + direction*perturbAmp/(1 + exp(-b*(mov_time-shift)));
target[i].thd = direction*perturbAmp*b*exp(-b*(mov_time-shift))/pow(1+exp(-b*(mov_time-shift)), 2);
target[i].thdd = direction*perturbAmp*(pow(b,2)*exp(-b*(mov_time-shift))/pow(1+exp(-b*(mov_time-shift)),2))*(2*exp(-b*(mov_time-shift))/(1+exp(-b*(mov_time-shift)))-1);
*/
// position-servo
/* Frequency of playback:
if 1point/s => k = (int)(task_time);
if 10 points/s => k = (int)(task_time*10);
if 420 points /s => k = (int)(task_time*420);
*/
k = (int)(420*task_time); //420 Hz sampling
if (k > ARRAYLEN-1){
k = ARRAYLEN-1; // to prevent index out of Array
}
// printf("task_time = %i, %f\n",k, posArray[k]);
//printf("task_time = %i, %f\n",k, pos2DArray[k][1]);
target[R_EB].th = pos2DArray[k][1];
// }
// the following variables need to be assigned
for (i=1; i<=N_DOFS; ++i)
{
joint_des_state[i].th = target[i].th;
joint_des_state[i].thd = target[i].thd;
joint_des_state[i].thdd = target[i].thdd;
//joint_des_state[i].uff = 0.0;
}
// compute inverse dynamics torques
SL_InvDynNE(joint_state,joint_des_state,endeff,&base_state,&base_orient);
return TRUE;
}
/*****************************************************************************
******************************************************************************
Function Name : run_gravityComp_task
Date : Dec. 1997
Remarks:
run the task from the task servo: REAL TIME requirements!
******************************************************************************
Paramters: (i/o = input/output)
none
*****************************************************************************/
static int
run_gravityComp_task(void)
{
int j,i;
// Damping Gains (index starts at 1)
//double Kdamp[N_DOFS+1] = {0.0, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.1, 0.1, 0.1};
double Kdamp[N_DOFS+1] = {0.0, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.07, 0.1, 0.1, 0.1};
// double Kdamp[N_DOFS+1] = {0.0, 0.2, 0.2, 0.5, 0.5, 1.0, 0.5, 0.5, 1, 1, 1};
// Coriolus Compensation Gains
double Kcori[N_DOFS+1] = {0.0, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 0.00, 0.00, 0.00};
// double Kcori[N_DOFS+1] = {0.0, 0.30, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.00, 0.00, 0.00};
// double Kcori[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};
// Inertia Compensation Gains
double Kiner[N_DOFS+1] = {0.0, 0.40, 0.40, 0.30, 0.35, 0.00, 0.4, 0.4, 0.00, 0.00, 0.00};
// double Kiner[N_DOFS+1] = {0.0, 1.00, 1.00, 1.00, 1.00, 0.50, 1.00, 1.00, 0.00, 0.00, 0.00};
//double Kiner[N_DOFS+1] = {0.0, 0.20, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, 0.00, 0.00, 0.00};
// double Kiner[N_DOFS+1] = {0.0, 0.50, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.00, 0.00, 0.00};
// double Kiner[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.0, 0.0, 0.00, 0.00, 0.00};
// Integer gains (how fast desired state becomes actual state)
//double Kint[N_DOFS+1] = {0.0, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.001, 0.001, 0.001};
double Kint[N_DOFS+1] = {0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.001, 0.001, 0.001};
//double Kint[N_DOFS+1] = {0.0, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.001, 0.001, 0.001};
// Dead Zone Threshold
double thres[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.05, 0.05, 0.05};
// To shut off Inertia,Coriolus Compensation
//double Kcori[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};
//double Kiner[N_DOFS+1] = {0.0, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};
for (i=1; i<=N_DOFS; ++i) {
joint_filt_state[i] = filter(i);
}
if (joint_state[8].th < 0.08) { // Hold down thumb to trigger Grav. Comp.
// Gravity Compensation
for (i=1; i<=N_DOFS; ++i) {
if (fabs(joint_state[i].th-joint_des_state[i].th) > thres[i]){
joint_des_state[i].th += (joint_state[i].th-joint_des_state[i].th)*Kint[i];
}
}
// add compensation relative to the current state of the robot
for (i=1; i<=N_DOFS; ++i) {
// Gravity Compensation
target[i].th = joint_state[i].th;
// Coriolus Compensation
target[i].thd = Kcori[i]*joint_filt_state[i].thd;
// Inertia Compensation
target[i].thdd = Kiner[i]*joint_filt_state[i].thdd;
// Damping (to be premultiplied by inertia matrix)
// if (i != 5 ) {
// target[i].thdd += (0.0 - joint_state[i].thd ) * Kdamp[i] * DampScale * controller_gain_thd[i];
// }
target[i].uff = 0.0;
}
SL_InvDynNE(NULL,target,endeff,&base_state,&base_orient);
// assign compensation torques
for (i=1; i<=N_DOFS; ++i) {
if (i==1 || i==2 || i==3 || i==4 || i==6 || i==7 || i==5) {
// Use my own damping (not premultiplied by the inertia matrix)
joint_des_state[i].uff =
target[i].uff + (0.0 - joint_filt_state[i].thd) * Kdamp[i] * DampScale * controller_gain_thd[i];
// Cancel damping in the motor servo
joint_des_state[i].thd = joint_state[i].thd;
}
else {
// Do not use my own damping
joint_des_state[i].uff = target[i].uff;
}
}
fieldtorque = 0.0;
/* FORCE FIELDS GO HERE */
switch (fieldon) {
case 1:
joint_des_state[4].uff += joint_filt_state[2].thd * fieldGain + joint_filt_state[1].thd * fieldGain;
break;
case 2:
if (joint_state[1].thd > 0.0) {
joint_des_state[5].uff += joint_state[1].thd * -0.5;
}
break;
case 3:
joint_des_state[2].uff += joint_state[3].thd * 1.0;
break;
case 4:
joint_des_state[1].uff += joint_state[4].thd * 1.0;
break;
case 5:
joint_des_state[4].uff += joint_state[3].thd * -1.0;
break;
case 6:
if (joint_state[1].thd > 0.0) {
joint_des_state[3].uff += joint_state[1].thd * 1.0;
}
break;
case 7: //end-effector:
for (i=1;i<=7;i++) {
for (j=1;j<=3;j++) {
J7T[i][j] = J[j][i];
}
}
for (i=1;i<=3;i++) {
xd[i] = cart_state[1].xd[i];
}
if (!mat_mult(J7T,B, J7T))
return FALSE;
if (!mat_vec_mult(J7T, xd, u_field))
return FALSE;
for (i=1;i<=7;i++) {
joint_des_state[i].uff += fieldGain*u_field[i];
}
fieldtorque = fieldGain*u_field[1];
break;
}
}
return TRUE;
}
