/*!*****************************************************************************
*******************************************************************************
\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_sim_state
\date Nov. 2007
\remarks
recieves the entire joint_sim_state from shared memory
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static int
receive_sim_state(void)
{
int i;
if (semTake(sm_joint_sim_state_sem,ns2ticks(TIME_OUT_NS)) == ERROR) {
++motor_servo_errors;
return FALSE;
}
for (i=1; i<=n_dofs; ++i)
sm_joint_sim_state_data[i] = sm_joint_sim_state->joint_sim_state[i];
cSL_Jstate(joint_sim_state,sm_joint_sim_state_data,n_dofs,FLOAT2DOUBLE);
// get time stamp and check for synchronization errors
motor_servo_time = servo_time = sm_joint_sim_state->ts;
semGive(sm_joint_sim_state_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 send_real_state
\date Nov. 2007
\remarks
sends the entire joint_state to shared memory (used for the openGL)
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int updateOpenGL(SL_Jstate *j_state, double* misc)
{
// joint state
if (semTake(sm_joint_sim_state_sem,ns2ticks(NO_WAIT)) == ERROR)
{
return FALSE;
}
cSL_Jstate(j_state,sm_joint_sim_state_data,n_dofs,DOUBLE2FLOAT);
for (int i=1; i<=n_dofs; ++i)
sm_joint_sim_state->joint_sim_state[i] = sm_joint_sim_state_data[i];
semGive(sm_joint_sim_state_sem);
// // base state
// if (semTake(sm_base_state_sem,ns2ticks(NO_WAIT)) == ERROR)
// {
// return FALSE;
// }
//
// 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)
// {
// return FALSE;
//
// }
// SL_quat imu_orient_quat;
// imu_orient_quat.q[_Q0_] = misc[B_Q0_IMU];
// imu_orient_quat.q[_Q1_] = misc[B_Q1_IMU];
// imu_orient_quat.q[_Q2_] = misc[B_Q2_IMU];
// imu_orient_quat.q[_Q3_] = misc[B_Q3_IMU];
//
// cSL_quat(&imu_orient_quat-1, sm_base_orient_data, 1, DOUBLE2FLOAT);
//
// sm_base_orient->orient[1] = sm_base_orient_data[1];
//
// semGive(sm_base_orient_sem);
//misc sensors
if (semTake(sm_misc_sim_sensor_sem,ns2ticks(NO_WAIT)) == ERROR)
{
return FALSE;
}
for (int i=1; i<=n_misc_sensors; ++i)
sm_misc_sim_sensor->value[i] = misc_sensor[i];
semGive(sm_misc_sim_sensor_sem);
//contacts
if (semTake(sm_contacts_sem,ns2ticks(NO_WAIT)) == ERROR) {
return FALSE;
}
//TO BE IMPLEMENTED
for (int i=0; i<=n_links; ++i)
{
sm_contacts->contact[i].status = 0; //contacts[i].status;
for (int j=1; j<=N_CART; ++j)
sm_contacts->contact[i].f[j] = 0; //contacts[i].f[j];
}
semGive(sm_contacts_sem);
return TRUE;
}
