static int
read_simulated_base(void)
{
// base state
if (semTake(sm_base_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR)
return FALSE;
sm_base_state_data[1] = sm_base_state->state[1];
cSL_Cstate(&base_state-1, sm_base_state_data, 1, FLOAT2DOUBLE);
semGive(sm_base_state_sem);
// base orient
if (semTake(sm_base_orient_sem,ns2ticks(TIME_OUT_NS)) == ERROR)
return FALSE;
sm_base_orient_data[1] = sm_base_orient->orient[1];
cSL_quat(&base_orient-1, sm_base_orient_data, 1, FLOAT2DOUBLE);
semGive(sm_base_orient_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note send_sim_state
\date Nov. 2007
\remarks
sends the entire joint_sim_state to shared memory
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
send_sim_state(void)
{
int i;
// joint state
if (semTake(sm_joint_sim_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
cSL_Jstate(joint_sim_state,sm_joint_sim_state_data,n_dofs,DOUBLE2FLOAT);
for (i=1; i<=n_dofs; ++i)
sm_joint_sim_state->joint_sim_state[i] = sm_joint_sim_state_data[i];
sm_joint_sim_state->ts = simulation_servo_time;
semGive(sm_joint_sim_state_sem);
// base state
if (semTake(sm_base_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
cSL_Cstate((&base_state)-1, sm_base_state_data, 1, DOUBLE2FLOAT);
sm_base_state->state[1] = sm_base_state_data[1];
sm_base_state->ts = simulation_servo_time;
semGive(sm_base_state_sem);
// base orient
if (semTake(sm_base_orient_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++simulation_servo_errors;
return FALSE;
}
cSL_quat(&base_orient-1, sm_base_orient_data, 1, DOUBLE2FLOAT);
sm_base_orient->orient[1] = sm_base_orient_data[1];
sm_base_orient->ts = simulation_servo_time;
semGive(sm_base_orient_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note receive_ros_state
\date July 2010
\remarks
receives all relevant state variables to ROS process
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
receive_ros_state(void)
{
int i,j;
SL_fJstate *fJstate;
SL_fDJstate *fDJstate;
SL_fCstate *fCstate;
SL_fquat *fquat;
float *misc;
double ts;
int dticks;
if (semTake(sm_ros_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++ros_servo_errors;
return FALSE;
}
// create pointers to share memory
fJstate = (SL_fJstate *) sm_ros_state->data;
fDJstate = (SL_fDJstate *) &(sm_ros_state->data[sizeof(SL_fJstate)*(n_dofs+1)]);
fCstate = (SL_fCstate *) &(sm_ros_state->data[sizeof(SL_fJstate)*(n_dofs+1)+
sizeof(SL_fDJstate)*(n_dofs+1)]);
fquat = (SL_fquat *) &(sm_ros_state->data[sizeof(SL_fJstate)*(n_dofs+1)+
sizeof(SL_fDJstate)*(n_dofs+1)+
sizeof(SL_fCstate)*(1+1)]);
misc = (float *) &(sm_ros_state->data[sizeof(SL_fJstate)*(n_dofs+1)+
sizeof(SL_fDJstate)*(n_dofs+1)+
sizeof(SL_fCstate)*(1+1)+
sizeof(SL_fquat)*(1+1)]);
memcpy((void *)(&sm_joint_state_data[1]),(const void*)(&(fJstate[1])),sizeof(SL_fJstate)*n_dofs);
memcpy((void *)(&sm_joint_des_state_data[1]),(const void*)(&(fDJstate[1])),sizeof(SL_fDJstate)*n_dofs);
memcpy((void *)(&sm_base_state_data[1]),(const void*)(&(fCstate[1])),sizeof(SL_fCstate)*1);
memcpy((void *)(&sm_base_orient_data[1]),(const void*)(&(fquat[1])),sizeof(SL_fquat)*1);
memcpy((void *)(&sm_misc_sensor_data[1]),(const void*)(&(misc[1])),sizeof(float)*n_misc_sensors);
cSL_Jstate(joint_state,sm_joint_state_data,n_dofs,FLOAT2DOUBLE);
cSL_DJstate(joint_des_state, sm_joint_des_state_data, n_dofs,FLOAT2DOUBLE);
cSL_Cstate((&base_state)-1, sm_base_state_data, 1, FLOAT2DOUBLE);
cSL_quat((&base_orient)-1, sm_base_orient_data, 1, FLOAT2DOUBLE);
for (i=1; i<=n_misc_sensors; ++i)
misc_sensor[i] = (double) sm_misc_sensor_data[i];
// get time stamp and adjust time
ros_servo_time = servo_time = sm_ros_state->ts;
semGive(sm_ros_state_sem);
return TRUE;
}
/*!*****************************************************************************
*******************************************************************************
\note run_user_task
\date Nov. 2007
\remarks
this function is clocked out of the task servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
run_user_task(void)
{
int i,j;
MY_MATRIX(M,1,N_CART,1,N_CART);
MY_VECTOR(v,1,N_CART);
// compute the contact forces in world coordinates at the feet from the
// force sensors
/* rotation matrix from world to L_AAA coordinates:
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
#ifdef HAS_LOWER_BODY
// transform forces
v[_X_] = misc_sensor[L_CFx];
v[_Y_] = misc_sensor[L_CFy];
v[_Z_] = misc_sensor[L_CFz];
mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].cf);
// transform torques
v[_A_] = misc_sensor[L_CTa];
v[_B_] = misc_sensor[L_CTb];
v[_G_] = misc_sensor[L_CTg];
mat_vec_mult_size(Alink[L_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[LEFT_FOOT].ct);
/* rotation matrix from world to R_AAA coordinates :
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
// transform forces
// transform forces
v[_X_] = misc_sensor[R_CFx];
v[_Y_] = misc_sensor[R_CFy];
v[_Z_] = misc_sensor[R_CFz];
mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].cf);
// transform torques
v[_A_] = misc_sensor[R_CTa];
v[_B_] = misc_sensor[R_CTb];
v[_G_] = misc_sensor[R_CTg];
mat_vec_mult_size(Alink[R_IN_HEEL],N_CART,N_CART,v,N_CART,endeff[RIGHT_FOOT].ct);
#endif
differentiate_cog(&cog);
test_fall(&cog);
// use the simulated base state if required
if (use_simulated_base_state)
read_simulated_base();
else
{
// do base state estimation
// update_base_state_estimation(&base_orient, &base_state);
// run_state_est_lin_task();
// getPelv(&base_orient, &base_state);
// base state
if (semTake(sm_base_state_sem,ns2ticks(NO_WAIT)) == ERROR)
{
//printf("sm_base_state_sem take error\n");
return TRUE;
}
cSL_Cstate((&base_state)-1, sm_base_state_data, 1, DOUBLE2FLOAT);
sm_base_state->state[1] = sm_base_state_data[1];
semGive(sm_base_state_sem);
// base orient
if (semTake(sm_base_orient_sem,ns2ticks(NO_WAIT)) == ERROR)
{
//printf("sm_base_orien_sem take error\n");
return TRUE;
}
cSL_quat(&base_orient-1, sm_base_orient_data, 1, DOUBLE2FLOAT);
sm_base_orient->orient[1] = sm_base_orient_data[1];
semGive(sm_base_orient_sem);
}
return TRUE;
}
