how to control same joint on different part like left_arm & right_arm

[jimingda]

I have a simple question that if i want to control the joint like joint 3(elbow) on both arms left_arm & right_arm, how can i call functions like setPositions && setControlModes, since these functions only differentiate joints but not distinguish left or right arm.

I try to find some demo codes relate to this problem but failed, may i didnt find the right website.

Thanks in advance.

[pattacini]

Hi @jimingda

You ought to simply open 2 device drivers, one for the left and one for the right arm. Then, you'll have to duplicate the calls.

For example, tutorial_joint-interface opens up only one device driver to control the left arm.
See https://github.com/vvv-school/tutorial_joint-interface/blob/master/src/position.cpp#L84-L121.

There are ways to open just one single device to control all the robot joints using ad-hoc remappers. However, this is more advanced and it is not available on all the robots.

[jimingda]

Thanks @pattacini

I tried in vvv-school/tutorial_joint-interface positiondirect demo, added another Property optJoint2 connecting right_arm after optJoint and another PolyDriver clienJoint2 opening optJoint2, but only right arm respond to the 'go' cmd. If i want the cmd 'go' set position command to both arms, what else do i need to modify or add.

Shall i need to open another rpc port attaching the same port '/positiondirect', or use the current rpc and just modify the respond or go function (set parameter to both arms), but i didn't find any parameter in function respond and go that related to which arm to configure.

[pattacini]

Hi @jimingda

Find below a possible implementation.
I didn't compile it though so there might be errors.

// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
//
// A tutorial on how to use the Gaze Interface.
//
// Author: Ugo Pattacini - <[email protected]>

#include <cstdlib>
#include <mutex>
#include <condition_variable>
#include <cmath>

#include <yarp/os/Network.h>
#include <yarp/os/LogStream.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/RpcServer.h>
#include <yarp/os/Time.h>

#include <yarp/dev/Drivers.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/ControlBoardInterfaces.h>

using namespace std;
using namespace yarp::os;
using namespace yarp::dev;


class CtrlModule: public RFModule
{
protected:
 PolyDriver clienJoint_l, clienJoint_r;
 IControlLimits *ilim_l, *ilim_r;
 IEncoders *ienc_l, *ienc_r;
 IControlMode *imod_l, *imod_r;
 IPositionDirect *iposd_l, *iposd_r;

 RpcServer rpc;

 mutex mtx_done;
 condition_variable cv_done;

 int joint;
 double target;
 bool control;
 double enc0;
 double t0;

 void go() {
   // retrieve joint bounds
   double min, max, range;
   ilim_l->getLimits(joint, &min, &max);
   range = max - min;

   // retrieve current joint position
   double enc;
   ienc_l->getEncoder(joint, &enc);

   // select target
   if (fabs(enc - min) < fabs(enc - max))
     target = max - 0.1 * range;
   else
     target = min + 0.1 * range;

   // set control mode
   imod_l->setControlMode(joint, VOCAB_CM_POSITION_DIRECT);
   imod_r->setControlMode(joint, VOCAB_CM_POSITION_DIRECT);

   // start the control
   yInfo() << "Yielding new target: " << target << " [deg]";
   t0 = Time::now();
   enc0 = enc;
   control = true;

   // wait until we're done
   unique_lock<mutex> lck(mtx_done);
   cv_done.wait(lck);
   yInfo() << "We're done";
    }

public:
    virtual bool configure(ResourceFinder &rf)
    {
        // open a client interface to connect to the joint controller
        Property optJoint_l, optJoint_r;
        optJoint_l.put("device","remote_controlboard");
        optJoint_l.put("remote", "/icubSim/left_arm");
        optJoint_l.put("local", "/positiondirect/left_arm");

        optJoint_r.put("device", "remote_controlboard");
        optJoint_r.put("remote", "/icubSim/right_arm");
        optJoint_r.put("local", "/positiondirect/right_arm");

        if (!clienJoint.open(optJoint_l)) {
            yError() << "Unable to connect to /icubSim/left_arm";
            return false;
        }

        if (!clienJoint.open(optJoint_r)) {
            yError() << "Unable to connect to /icubSim/right_arm";
            clienJoint_l.close();
            return false;
        }

        // open views
        bool ok=true;
        ok=ok && clienJoint_l.view(ilim_l) && clienJoint_r.view(ilim_r);
        ok=ok && clienJoint_l.view(ienc_l) && clienJoint_r.view(ienc_r);
        ok=ok && clienJoint_l.view(imod_l) && clienJoint_r.view(imod_r);
        ok=ok && clienJoint_l.view(iposd_l) && clienJoint_r.view(iposd_r);

        if (!ok)
        {
            yError()<<"Unable to open views";
            return false;
        }

        // elbow
        joint=3;

        // start with control disabled
        control=false;

        // open rpc port
        rpc.open("/positiondirect");

        // attach the callback respond()
        attach(rpc);

        return true;
    }

    virtual bool close()
    {
        rpc.close();
        clienJoint_l.close();
        clienJoint_r.close();

        return true;
    }

    virtual bool respond(const Bottle &cmd, Bottle &reply)
    {
        if (cmd.get(0).asString()=="go")
        {
            go();
            reply.addString("ack");
        }
        else if (cmd.get(0).asString()=="enc")
        {
            double enc_l, enc_r;
            ienc_l->getEncoder(joint,&enc_l);
            ienc_r->getEncoder(joint,&enc_r);
            reply.addString("ack");
            reply.addFloat64(enc_l);
            reply.addFloat64(enc_r);
        }
        else
            reply.addString("nack");

        return true;
    }

    virtual double getPeriod()
    {
        // we set up the control rate here in [s]
        return 0.02;
    }

    virtual bool updateModule()
    {
        if (control)
        {
            double T=5.0;   // time period in [s]
            double dt=Time::now()-t0;

            // time ratios
            double r=dt/T;
            double r3=r*r*r;
            double r4=r3*r;
            double r5=r4*r;

            // minimum-jerk formula
            double ref=enc0+(target-enc0)*(10.0*r3-15.0*r4+6.0*r5);
            iposd_l->setPosition(joint,ref);
            iposd_r->setPosition(joint,ref);

            // notify we're done
            if (dt>T)
            {
                control=false;
                cv_done.notify_all();
            }
        }

        return true;
    }
};



int main(int argc, char *argv[])
{
    Network yarp;
    if (!yarp.checkNetwork())
    {
        yError()<<"YARP doesn't seem to be available";
        return EXIT_FAILURE;
    }

    ResourceFinder rf;
    rf.configure(argc,argv);

    CtrlModule mod;
    return mod.runModule(rf);
}

[jimingda]

Thank you @pattacini the code works very well. Another question is that what if i want to control multi joints on the same arm, like shoulder and elbow (joint2 and joint3) by one single command, I check the yarp tutorials about getEncoders and setPositions, according to my understanding, does getEncoders return all joints's angles while setPositions can be assigned specific joints, right?

[pattacini]

The method setPositions() is overloaded and one of its implementations accepts a list defining which joints of the specific part to control.

Here's an example:

vector<int> joints{0, 2, 3};
vector<double> refs{10., 10., -30.};
iposd->setPosition((int)joints.size(), joints.data(), refs.data());

[jimingda]

Thanks. I also want to set a series of positions to joints(the joints' positions is from samples by yarpdatadumper in compliant mode). Is the way to do it is to set those position simultaneously, and let yarp iKin or cartesiansolver plan and generate the trajectory path and run the low level command ?

[pattacini]

If you stored positions using yarpdatadumper, then you should replay those trajectories using the combination of yarpdataplayer and wholeBodyPlayer.

cc @Nicogene