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 - <ugo.pattacini@iit.it>

#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

[SuperDiodo]

Hi, sorry for reviving this discussion after so many years, but I'm also interested in controlling joints belonging to different iCub parts.

I found this example, which demonstrates how to control multiple joints as if they belonged to the same robot part using remotecontrolboardremapper.

I'm currently facing some issues with this approach (see this issue). However, apart from that problem, it seems to work reasonably well.

I was wondering whether there are alternative approaches to achieve the same goal. Is remotecontrolboardremapper the recommended solution, or are there other ways to control joints across multiple robot parts without having to instantiate separate control interfaces (e.g., IPositionControl, IPositionDirect, etc.) for each robot part?

[Nicogene]

Hello @SuperDiodo, remotecontrolboardremapper is definitely a valid approach.
Alternatively, if you can use a controlboardremapper and then make that a controlBoard_nws_yarp attach it. On user code you can connect to that controlBoard_nws_yarp via a simple remote_controlboard, but as you can see it is different form of what you already accomplished with the remotecontrolboardremapper

In case, you can take inspiration from what we are doing with ergoCub:

• https://github.com/robotology/robots-configuration/blob/master/ergoCubSN001/wrappers/motorControl/alljoints-mc_remapper.xml

The segmentation fault is probably due to the attach/detach level specified in the device xml

[SuperDiodo]

Thanks, I'll have a look at it.

FYI, I've updated the issue with the results of some additional investigations. I tested the code on a slightly newer iCub (still an older robot, but one that is currently supported, unlike the one I used yesterday), and the problem has been narrowed down to a single method that consistently triggers the segmentation fault in yarprobotinterface.