void sendContacts(BufferedPort<skinContactList> &outPort, const skinContactList &sCL)
    {
        ts.update();
        skinContactList &sCLout = outPort.prepare();
        sCLout.clear();

        sCLout = sCL;
        
        outPort.setEnvelope(ts);
        outPort.write();     
    }
Пример #2
0
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);
        }
    }
}
Пример #3
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");
}