void sendContacts(BufferedPort<skinContactList> &outPort, const skinContactList &sCL)
{
ts.update();
skinContactList &sCLout = outPort.prepare();
sCLout.clear();
sCLout = sCL;
outPort.setEnvelope(ts);
outPort.write();
}
void gravityCompensatorThread::run()
{
thread_status = STATUS_OK;
static int delay_check=0;
if(isCalibrated==true)
{
if (readAndUpdate(false) == false)
{
delay_check++;
yWarning ("network delays detected (%d/10)\n", delay_check);
if (delay_check>=10)
{
yError ("gravityCompensatorThread lost connection with wholeBodyDynamics.\n");
thread_status = STATUS_DISCONNECTED;
}
}
else
{
delay_check = 0;
}
Vector F_up(6,0.0);
icub->upperTorso->setInertialMeasure(w0,dw0,d2p0);
icub->upperTorso->solveKinematics();
icub->upperTorso->solveWrench();
//compute the arm torques
Matrix F_sens_up = icub->upperTorso->estimateSensorsWrench(F_ext_up,false);
gravity_torques_LA = icub->upperTorso->left->getTorques();
gravity_torques_RA = icub->upperTorso->right->getTorques();
//compute the torso torques
icub->attachLowerTorso(F_up,F_up);
icub->lowerTorso->solveKinematics();
icub->lowerTorso->solveWrench();
Vector tmp; tmp.resize(3);
tmp = icub->lowerTorso->getTorques("torso");
gravity_torques_TO[0] = tmp [2];
gravity_torques_TO[1] = tmp [1];
gravity_torques_TO[2] = tmp [0];
//compute the leg torques
Matrix F_sens_low = icub->lowerTorso->estimateSensorsWrench(F_ext_low,false);
gravity_torques_LL = icub->lowerTorso->getTorques("left_leg");
gravity_torques_RL = icub->lowerTorso->getTorques("right_leg");
//#define DEBUG_TORQUES
#ifdef DEBUG_TORQUES
yDebug ("TORQUES: %s *** \n\n", torques_TO.toString().c_str());
yDebug ("LL TORQUES: %s *** \n\n", torques_LL.toString().c_str());
yDebug ("RL TORQUES: %s *** \n\n", torques_RL.toString().c_str());
#endif
//read the external command ports
Vector *offset_input_la = la_additional_offset->read(false);
if(offset_input_la!=0)
{
int size = (offset_input_la->size() < 7) ? offset_input_la->size():7;
for (int i=0; i<size; i++)
{externalcmd_torques_LA[i]=(*offset_input_la)[i];}
}
Vector *offset_input_ra = ra_additional_offset->read(false);
if(offset_input_ra!=0)
{
int size = (offset_input_ra->size() < 7) ? offset_input_ra->size():7;
for (int i=0; i<size; i++)
{externalcmd_torques_RA[i]=(*offset_input_ra)[i];}
}
Vector *offset_input_ll = ll_additional_offset->read(false);
if(offset_input_ll!=0)
{
int size = (offset_input_ll->size() < 6) ? offset_input_ll->size():6;
for (int i=0; i<size; i++)
{externalcmd_torques_LL[i]=(*offset_input_ll)[i];}
}
Vector *offset_input_rl = rl_additional_offset->read(false);
if(offset_input_rl!=0)
{
int size = (offset_input_rl->size() < 6) ? offset_input_rl->size():6;
for (int i=0; i<size; i++)
{externalcmd_torques_RL[i]=(*offset_input_rl)[i];}
}
Vector *offset_input_to = to_additional_offset->read(false);
if(offset_input_to!=0)
{
int size = (offset_input_to->size() < 3) ? offset_input_to->size():3;
for (int i=0; i<size; i++)
{externalcmd_torques_TO[i]=(*offset_input_to)[i];}
}
//compute the command to be given to the joint
if (gravity_mode==GRAVITY_COMPENSATION_ON)
{
if (external_mode==EXTERNAL_TRQ_ON)
{
exec_torques_LA = ampli_LA*gravity_torques_LA + externalcmd_torques_LA;
exec_torques_RA = ampli_RA*gravity_torques_RA + externalcmd_torques_RA;
exec_torques_LL = ampli_LL*gravity_torques_LL + externalcmd_torques_LL;
exec_torques_RL = ampli_RL*gravity_torques_RL + externalcmd_torques_RL;
exec_torques_TO = ampli_TO*gravity_torques_TO + externalcmd_torques_TO;
}
else
{
exec_torques_LA = ampli_LA*gravity_torques_LA;
exec_torques_RA = ampli_RA*gravity_torques_RA;
exec_torques_LL = ampli_LL*gravity_torques_LL;
exec_torques_RL = ampli_RL*gravity_torques_RL;
exec_torques_TO = ampli_TO*gravity_torques_TO;
}
}
else
{
if (external_mode==EXTERNAL_TRQ_ON)
{
exec_torques_LA = externalcmd_torques_LA;
exec_torques_RA = externalcmd_torques_RA;
exec_torques_LL = externalcmd_torques_LL;
exec_torques_RL = externalcmd_torques_RL;
exec_torques_TO = externalcmd_torques_TO;
}
else
{
externalcmd_torques_LA.zero();
externalcmd_torques_RA.zero();
externalcmd_torques_LL.zero();
externalcmd_torques_RL.zero();
externalcmd_torques_TO.zero();
}
}
//execute the commands
static yarp::os::Stamp timestamp;
timestamp.update();
if (iCtrlMode_arm_left)
{
feedFwdGravityControl(left_arm_ctrlJnt, "left_arm", iCtrlMode_arm_left,iTqs_arm_left,iImp_arm_left,iIntMode_arm_left,exec_torques_LA);
if (left_arm_exec_torques && left_arm_exec_torques->getOutputCount()>0)
{
left_arm_exec_torques->prepare() = exec_torques_LA;
left_arm_exec_torques->setEnvelope(timestamp);
left_arm_exec_torques->write();
}
if (left_arm_gravity_torques && left_arm_gravity_torques->getOutputCount()>0)
{
left_arm_gravity_torques->prepare() = gravity_torques_LA;
left_arm_gravity_torques->setEnvelope(timestamp);
left_arm_gravity_torques->write();
}
}
if (iCtrlMode_arm_right)
{
feedFwdGravityControl(right_arm_ctrlJnt, "right_arm", iCtrlMode_arm_right,iTqs_arm_right,iImp_arm_right,iIntMode_arm_right,exec_torques_RA);
if (right_arm_exec_torques && right_arm_exec_torques->getOutputCount()>0)
{
right_arm_exec_torques->prepare() = exec_torques_RA;
right_arm_exec_torques->setEnvelope(timestamp);
right_arm_exec_torques->write();
}
if (right_arm_gravity_torques && right_arm_gravity_torques->getOutputCount()>0)
{
right_arm_gravity_torques->prepare() = gravity_torques_RA;
right_arm_gravity_torques->setEnvelope(timestamp);
right_arm_gravity_torques->write();
}
}
if (iCtrlMode_torso)
{
feedFwdGravityControl(torso_ctrlJnt, "torso", iCtrlMode_torso,iTqs_torso,iImp_torso,iIntMode_torso,exec_torques_TO);
if (torso_exec_torques && torso_exec_torques->getOutputCount()>0)
{
torso_exec_torques->prepare() = exec_torques_TO;
torso_exec_torques->setEnvelope(timestamp);
torso_exec_torques->write();
}
if (torso_gravity_torques && torso_gravity_torques->getOutputCount()>0)
{
torso_gravity_torques->prepare() = gravity_torques_TO;
torso_gravity_torques->setEnvelope(timestamp);
torso_gravity_torques->write();
}
}
if (iCtrlMode_leg_left)
{
feedFwdGravityControl(left_leg_ctrlJnt, "left_leg", iCtrlMode_leg_left,iTqs_leg_left,iImp_leg_left,iIntMode_leg_left,exec_torques_LL);
if (left_leg_exec_torques && left_leg_exec_torques->getOutputCount()>0)
{
left_leg_exec_torques->prepare() = exec_torques_LL;
left_leg_exec_torques->setEnvelope(timestamp);
left_leg_exec_torques->write();
}
if (left_leg_gravity_torques && left_leg_gravity_torques->getOutputCount()>0)
{
left_leg_gravity_torques->prepare() = gravity_torques_LL;
left_leg_gravity_torques->setEnvelope(timestamp);
left_leg_gravity_torques->write();
}
}
if (iCtrlMode_leg_right)
{
feedFwdGravityControl(right_leg_ctrlJnt, "right_leg", iCtrlMode_leg_right,iTqs_leg_right,iImp_leg_right,iIntMode_leg_right,exec_torques_RL);
if (right_leg_exec_torques && right_leg_exec_torques->getOutputCount()>0)
{
right_leg_exec_torques->prepare() = exec_torques_RL;
right_leg_exec_torques->setEnvelope(timestamp);
right_leg_exec_torques->write();
}
if (right_leg_gravity_torques && right_leg_gravity_torques->getOutputCount()>0)
{
right_leg_gravity_torques->prepare() = gravity_torques_RL;
right_leg_gravity_torques->setEnvelope(timestamp);
right_leg_gravity_torques->write();
}
}
}
else
{
if(Network::exists(string("/"+wholeBodyName+"/filtered/inertial:o").c_str()))
{
yInfo("connection exists! starting calibration...\n");
//the following delay is required because even if the filtered port exists, may be the
//low pass filtered values have not reached yet the correct value.
Time::delay(1.0);
isCalibrated = true;
Network::connect(string("/"+wholeBodyName+"/filtered/inertial:o").c_str(),"/gravityCompensator/inertial:i");
setZeroJntAngVelAcc();
setUpperMeasure();
setLowerMeasure();
readAndUpdate(true);
icub->upperTorso->setInertialMeasure(w0,dw0,d2p0);
Matrix F_sens_up = icub->upperTorso->estimateSensorsWrench(F_ext_up,false);
icub->lowerTorso->setInertialMeasure(icub->upperTorso->getTorsoAngVel(),
icub->upperTorso->getTorsoAngAcc(),
icub->upperTorso->getTorsoLinAcc());
Matrix F_sens_low = icub->lowerTorso->estimateSensorsWrench(F_ext_low,false);
gravity_torques_LA = icub->upperTorso->getTorques("left_arm");
gravity_torques_RA = icub->upperTorso->getTorques("right_arm");
gravity_torques_LL = icub->lowerTorso->getTorques("left_leg");
gravity_torques_RL = icub->lowerTorso->getTorques("right_leg");
Vector LATorques = icub->upperTorso->getTorques("left_arm");
yDebug("encoders: %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf\n", encoders_arm_left(0), encoders_arm_left(1), encoders_arm_left(2), encoders_arm_left(3), encoders_arm_left(4), encoders_arm_left(5), encoders_arm_left(6));
yDebug("left arm: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_LA(0), gravity_torques_LA(1), gravity_torques_LA(2), gravity_torques_LA(3), gravity_torques_LA(4), gravity_torques_LA(5), gravity_torques_LA(6));
yDebug("right arm: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_RA(0), gravity_torques_RA(1), gravity_torques_RA(2), gravity_torques_RA(3), gravity_torques_RA(4), gravity_torques_RA(5), gravity_torques_RA(6));
yDebug("left leg: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_LL(0), gravity_torques_LL(1), gravity_torques_LL(2), gravity_torques_LL(3), gravity_torques_LL(4), gravity_torques_LL(5));
yDebug("right leg: %.3lf, %.3lf, %.3lf, %.3lf, %.3lf, %.3lf\n", gravity_torques_RL(0), gravity_torques_RL(1), gravity_torques_RL(2), gravity_torques_RL(3), gravity_torques_RL(4), gravity_torques_RL(5));
yDebug("inertial: %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf, %.1lf\n", d2p0(0), d2p0(1), d2p0(2), w0(0), w0(1), w0(2), dw0(0), dw0(1), dw0(2));
}
else
{
yInfo("waiting for connections from wholeBodyDynamics (port: %s)...\n", wholeBodyName.c_str());
Time::delay(1.0);
}
}
}
void yarp::dev::ServerInertial::run()
{
double before, now;
yInfo("Starting server Inertial thread\n");
while (!isStopping())
{
before = yarp::os::Time::now();
if (IMU!=NULL)
{
Bottle imuData;
bool res = getInertial(imuData);
// publish data on YARP port if required
if(useROS != ROS_only)
{
yarp::os::Bottle& bot = writer.get();
bot = imuData;
if (res)
{
static yarp::os::Stamp ts;
if (iTimed)
ts=iTimed->getLastInputStamp();
else
ts.update();
curr_timestamp_counter = ts.getCount();
if (curr_timestamp_counter!=prev_timestamp_counter)
{
if (!spoke)
{
yDebug("Writing an Inertial measurement.\n");
spoke = true;
}
p.setEnvelope(ts);
writer.write(strict);
}
else
{
trap++;
}
prev_timestamp_counter = curr_timestamp_counter;
}
}
// publish ROS topic if required
if(useROS != ROS_disabled)
{
double euler_xyz[3], quaternion[4];
euler_xyz[0] = imuData.get(0).asDouble();
euler_xyz[1] = imuData.get(1).asDouble();
euler_xyz[2] = imuData.get(2).asDouble();
convertEulerAngleYXZdegrees_to_quaternion(euler_xyz, quaternion);
sensor_msgs_Imu &rosData = rosPublisherPort.prepare();
rosData.header.seq = rosMsgCounter++;
rosData.header.stamp = normalizeSecNSec(yarp::os::Time::now());
rosData.header.frame_id = frame_id;
rosData.orientation.x = quaternion[0];
rosData.orientation.y = quaternion[1];
rosData.orientation.z = quaternion[2];
rosData.orientation.w = quaternion[3];
rosData.orientation_covariance = covariance;
rosData.linear_acceleration.x = imuData.get(3).asDouble(); // [m/s^2]
rosData.linear_acceleration.y = imuData.get(4).asDouble(); // [m/s^2]
rosData.linear_acceleration.z = imuData.get(5).asDouble(); // [m/s^2]
rosData.linear_acceleration_covariance = covariance;
rosData.angular_velocity.x = imuData.get(6).asDouble(); // to be converted into rad/s (?) - verify with users
rosData.angular_velocity.y = imuData.get(7).asDouble(); // to be converted into rad/s (?) - verify with users
rosData.angular_velocity.z = imuData.get(8).asDouble(); // to be converted into rad/s (?) - verify with users
rosData.angular_velocity_covariance = covariance;
rosPublisherPort.write();
}
}
/// wait 5 ms.
now = yarp::os::Time::now();
if ((now - before) < period) {
double k = period-(now-before);
yarp::os::Time::delay(k);
}
}
yInfo("Server Intertial thread finished\n");
}