void SslConnector::handle(AMQFrame& frame) {
    bool notifyWrite = false;
    {
    Mutex::ScopedLock l(lock);
    frames.push_back(frame);
    //only ask to write if this is the end of a frameset or if we
    //already have a buffers worth of data
    currentSize += frame.encodedSize();
    if (frame.getEof()) {
        lastEof = frames.size();
        notifyWrite = true;
    } else {
        notifyWrite = (currentSize >= maxFrameSize);
    }
    /*
      NOTE: Moving the following line into this mutex block
            is a workaround for BZ 570168, in which the test
            testConcurrentSenders causes a hang about 1.5%
            of the time.  ( To see the hang much more frequently
            leave this line out of the mutex block, and put a
            small usleep just before it.)

            TODO mgoulish - fix the underlying cause and then
                            move this call back outside the mutex.
    */
    if (notifyWrite && !closed) aio->notifyPendingWrite();
    }
}
示例#2
0
void ChannelUpdateVisitor::_updateDrawTiles( const Compound* compound,
                                             const RenderContext& context )
{
    Frames frames;
    std::vector< co::ObjectVersion > frameIDs;
    const Frames& outputFrames = compound->getOutputFrames();
    for( FramesCIter i = outputFrames.begin(); i != outputFrames.end(); ++i)
    {
        Frame* frame = *i;

        if( !frame->hasData( _eye )) // TODO: filter: buffers, vp, eye
            continue;

        frames.push_back( frame );
        frameIDs.push_back( co::ObjectVersion( frame ));
    }

    const Channel* destChannel = compound->getInheritChannel();
    const TileQueues& inputQueues = compound->getInputTileQueues();
    for( TileQueuesCIter i = inputQueues.begin(); i != inputQueues.end(); ++i )
    {
        const TileQueue* inputQueue = *i;
        const TileQueue* outputQueue = inputQueue->getOutputQueue( context.eye);
        const UUID& id = outputQueue->getQueueMasterID( context.eye );
        LBASSERT( id != UUID::ZERO );

        ChannelFrameTilesPacket tilesPacket;
        tilesPacket.isLocal = (_channel == destChannel);
        tilesPacket.context = context;
        tilesPacket.tasks = compound->getInheritTasks() &
                            ( eq::fabric::TASK_CLEAR | eq::fabric::TASK_DRAW |
                              eq::fabric::TASK_READBACK );

        tilesPacket.queueVersion.identifier = id;
        tilesPacket.queueVersion.version = co::VERSION_FIRST; // eile: ???
        tilesPacket.nFrames = uint32_t( frames.size( ));
        _channel->send< co::ObjectVersion >( tilesPacket, frameIDs );

        _updated = true;
        LBLOG( LOG_TASKS ) << "TASK tiles " << _channel->getName() <<  " "
                           << &tilesPacket << std::endl;
    }
}
void ChannelUpdateVisitor::_updateDrawTiles( const Compound* compound,
                                             const RenderContext& context )
{
    Frames frames;
    co::ObjectVersions frameIDs;
    const Frames& outputFrames = compound->getOutputFrames();
    for( FramesCIter i = outputFrames.begin(); i != outputFrames.end(); ++i)
    {
        Frame* frame = *i;

        if( !frame->hasData( _eye )) // TODO: filter: buffers, vp, eye
            continue;

        frames.push_back( frame );
        frameIDs.push_back( co::ObjectVersion( frame ));
    }

    const Channel* destChannel = compound->getInheritChannel();
    const TileQueues& inputQueues = compound->getInputTileQueues();
    for( TileQueuesCIter i = inputQueues.begin(); i != inputQueues.end(); ++i )
    {
        const TileQueue* inputQueue = *i;
        const TileQueue* outputQueue = inputQueue->getOutputQueue( context.eye);
        const UUID& id = outputQueue->getQueueMasterID( context.eye );
        LBASSERT( id != UUID::ZERO );

        const bool isLocal = (_channel == destChannel);
        const uint32_t tasks = compound->getInheritTasks() &
                            ( eq::fabric::TASK_CLEAR | eq::fabric::TASK_DRAW |
                              eq::fabric::TASK_READBACK );

        _channel->send( fabric::CMD_CHANNEL_FRAME_TILES )
                << context << isLocal << id << tasks << frameIDs;
        _updated = true;
        LBLOG( LOG_TASKS ) << "TASK tiles " << _channel->getName() <<  " "
                           << std::endl;
    }
}
//#include <chainidsolver_constraint_vereshchagin.hpp>
int main()
{

    using namespace KDL;


    //Definition of kinematic chain
    //-----------------------------------------------------------------------------------------------//
    //Joint (const JointType &type=None, const double &scale=1, const double &offset=0,
    //       const double &inertia=0, const double &damping=0, const double &stiffness=0)
    Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
    Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);
    //Joint rotJoint1 = Joint(Joint::None,1,0,0.01);

    Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
    Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
    Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
    //Segment (const Joint &joint=Joint(Joint::None),
    //         const Frame &f_tip=Frame::Identity(), const RigidBodyInertia &I=RigidBodyInertia::Zero())
    Segment segment1 = Segment(rotJoint0, frame1);
    Segment segment2 = Segment(rotJoint1, frame2);

    // 	RotationalInertia (double Ixx=0, double Iyy=0, double Izz=0, double Ixy=0, double Ixz=0, double Iyz=0)
    RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
    //RigidBodyInertia (double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
    RigidBodyInertia inerSegment1(1.0, Vector(0.0, -0.2, 0.0), rotInerSeg1);
    RigidBodyInertia inerSegment2(1.0, Vector(0.0, -0.2, 0.0), rotInerSeg1);
    segment1.setInertia(inerSegment1);
    segment2.setInertia(inerSegment2);

    Chain chain;
    chain.addSegment(segment1);
    //chain.addSegment(segment2);
    //----------------------------------------------------------------------------------------------//

    //Definition of constraints and external disturbances
    //--------------------------------------------------------------------------------------//
    JntArray arrayOfJoints(chain.getNrOfJoints());
    //Constraint force matrix at the end-effector
    //What is the convention for the spatial force matrix; is it the same as in thesis?
    Vector constrainXLinear(0.0, 1.0, 0.0);
    Vector constrainXAngular(0.0, 0.0, 0.0);
    Vector constrainYLinear(0.0, 0.0, 0.0);
    Vector constrainYAngular(0.0, 0.0, 0.0);
    Vector constrainZLinear(0.0, 0.0, 0.0);
    Vector constrainZAngular(0.0, 0.0, 0.0);
    const Twist constraintForcesX(constrainXLinear, constrainXAngular);
    const Twist constraintForcesY(constrainYLinear, constrainYAngular);
    const Twist constraintForcesZ(constrainZLinear, constrainZAngular);
    Jacobian alpha(1);
    alpha.setColumn(0, constraintForcesX);
    //alpha.setColumn(1,constraintForcesY);
    //alpha.setColumn(2,constraintForcesZ);

    //Acceleration energy at  the end-effector
    JntArray betha(1); //set to zero
    betha(0) = 0.0;
    //betha(1) = 0.0;
    //betha(2) = 0.0;

    //arm root acceleration
    Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
    Vector angularAcc(0.0, 0.0, 0.0);
    Twist twist1(linearAcc, angularAcc);

    //external forces on the arm
    Vector externalForce1(0.0, 0.0, 0.0);
    Vector externalTorque1(0.0, 0.0, 0.0);
    Vector externalForce2(0.0, 0.0, 0.0);
    Vector externalTorque2(0.0, 0.0, 0.0);
    Wrench externalNetForce1(externalForce1, externalTorque1);
    Wrench externalNetForce2(externalForce2, externalTorque2);
    Wrenches externalNetForce;
    externalNetForce.push_back(externalNetForce1);
    //externalNetForce.push_back(externalNetForce2);
    //-------------------------------------------------------------------------------------//

    //solvers
    ChainFkSolverPos_recursive fksolver(chain);
    //ChainFkSolverVel_recursive fksolverVel(chain);
    int numberOfConstraints = 1;
    ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);

    // Initial arm position configuration/constraint
    Frame frameInitialPose;
    JntArray jointInitialPose(chain.getNrOfJoints());
    jointInitialPose(0) = M_PI/6.0; // initial joint0 pose
    //jointInitialPose(1) = M_PI/6.0;                          //initial joint1 pose, negative in clockwise

    //cartesian space/link accelerations
    Twist accLink0;
    Twist accLink1;
    Twists cartAcc;
    cartAcc.push_back(accLink0);
    //cartAcc.push_back(accLink1);

    //arm joint rate configuration and initial values are zero
    JntArray qDot(chain.getNrOfJoints());
    //arm joint accelerations returned by the solver
    JntArray qDotDot(chain.getNrOfJoints());
    
    //arm joint torques returned by the solver
    JntArray qTorque(chain.getNrOfJoints());

    arrayOfJoints(0) = jointInitialPose(0);
    //arrayOfJoints(1) = jointInitialPose(1);

    //Calculate joint values for each cartesian position
    Frame frameEEPose;
    Twist frameEEVel;
    double TimeConstant = 1; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
    double timeDelta = 0.002;
    bool status;

    Frame cartX1;
    Frame cartX2;
    Frames cartX;
    cartX.push_back(cartX1);
    cartX.push_back(cartX2);
    Twists cartXDot;
    cartXDot.push_back(accLink0);
    cartXDot.push_back(accLink1);

    Twists cartXDotDot;
    cartXDotDot.push_back(accLink0);
    cartXDotDot.push_back(accLink1);


    for (double t = 0.0; t <= 5*TimeConstant; t = t + timeDelta)
    {
        //at time t0, inputs are arrayOfjoints(0,pi/2.0), qdot(0,0)
        //qDot(0) = 0.5;
        // qDot(1) = 0.5;

        //printf("Inside the main loop %f  %f\n",qTorque(0), qTorque(1));
        

        //NOTE AZAMAT:what is qTorque? in implementation returned qTorque is constraint torque generated by the virtual constraint forces.
        // these torques are required to satify a virtual constraint and basically are real torques applied to joints to achieve
        // these constraint. But if additionally to the constraint generated torques we also had internal motor torques, then should not they
        //be added together?
        
        //qTorque(1) = qTorque(1)+internalJointTorque1;

        // In what frame of reference are the results expressed?

        status = constraintSolver.CartToJnt(arrayOfJoints, qDot, qDotDot, alpha, betha, externalNetForce, qTorque);
        if (status >= 0)
        {
            //For 2-dof arm
            //printf("%f          %f %f       %f %f       %f %f           %f %f           %f %f\n",t , arrayOfJoints(0), arrayOfJoints(1), qDot(0), qDot(1), qDotDot(0), qDotDot(1), qTorque(0), qTorque(1), frameEEPose.p.x(), frameEEPose.p.y());
            //printf("%f          %f %f       %f %f       %f %f\n", t, cartAcc[0].vel.x(), cartAcc[0].vel.y(), cartAcc[0].rot.z(), cartAcc[1].vel.x(), cartAcc[1].vel.y(), cartAcc[1].rot.z());

            //For 1-dof
            
            printf("Joint actual %f          %f       %f        %f            %f\n", t, arrayOfJoints(0), qDot(0), qDotDot(0), qTorque(0));
            std::cout<<std::endl;
        }
        constraintSolver.getLinkCartesianPose(cartX);
        constraintSolver.getLinkCartesianVelocity(cartXDot);
        constraintSolver.getLinkCartesianAcceleration(cartXDotDot);
        constraintSolver.getLinkAcceleration(cartAcc);
        printf("Cartesian acc local %f          %f       %f        %f\n", t, cartAcc[0].vel.x(), cartAcc[0].vel.y(), cartAcc[0].rot.z());
        printf("Cartesian actual %f          %f      %f     %f      %f\n", t, cartX[0].p.x(), cartX[0].p.y(), cartXDot[0].vel[0], cartXDot[0].vel[1]);
        std::cout<<std::endl;
        //printf("External torque %f\n", externalNetForce[0].torque.z());
        //Integration at the given "instanteneous" interval for joint position and velocity.
        //These will be inputs for the next time instant
        //actually here it should be a time interval and not time from the beginning, that is why timeDelta;
        arrayOfJoints(0) = arrayOfJoints(0) + (qDot(0) + qDotDot(0) * timeDelta / 2.0) * timeDelta;
        qDot(0) = qDot(0) + qDotDot(0) * timeDelta;
        //arrayOfJoints(1) = arrayOfJoints(1) + (qDot(1) + qDotDot(1)*timeDelta/2.0)*timeDelta;
        //qDot(1) = qDot(1)+qDotDot(1)*timeDelta;



        //For cartesian space control one needs to calculate from the obtained joint space value, new cartesian space poses.
        //Then based on the difference of the signal (desired-actual) we define a regulation function (controller)
        // this difference should be compensated either by constraint forces or by joint torques.
        // The current version of the algorithm assumes that we don't have control over torques, thus change in constraint forces
        //should be a valid option.
    }

    //torque1=[(m1+m2)a1^2+m2a2^2+2m2a1a2cos2]qDotDot1+[m2a2^2+m2a1a2cos2]qDotDot2-m2a1a2(2qDot1qDot2+qDot2^2)sin2+(m1+m2)ga1cos2+m2ga2cos(1+2)

    //torque2=[m2a2^2+m2a1a2cos2]qDotDot1+m2a2^2qDotDot2+m2a1a2qDot1^2sin2+m2ga2cos(1+2)

    return 0;
}