Implement MOVI command in streamingDeviceController app

[PeterBowman]

Currently WIP and awaiting tests on the real robot. Keep in mind that:

• BCC does not perform joint velocity limit checks on this mode. Since https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/161 is still not mature enough, I'd consider submitting a dirty patch to enforce software checks specifically for MOVI.
• This app sends stop requests (at least with spnav devices), which currently set the controller back to position mode. Another control mode preset would be required.

[PeterBowman]

I need to have IPositionDirect::getRefPosition(s) implemented in order to make limit checks work (https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/406ea2696086a70bd36d1d82d09b5f9debc4c869). Related:

• https://github.com/roboticslab-uc3m/openrave-yarp-plugins/issues/94
• https://github.com/roboticslab-uc3m/yarp-devices/issues/204

[PeterBowman]

I need to have IPositionDirect::getRefPosition(s) implemented in order to make limit checks work (406ea26).

Sorry, I actually shouldn't be doing this. These reference values are not streamed by controlboardwrapper2, which means that network latency will be introduced in the movi method. I'd pick the output of getEncoders instead.

[PeterBowman]

For the joint velocity check, the streaming rate of new position data is needed. ASWJ, we'd like to query the JMC for this value via yarp::dev::IRemoteVariables.

[PeterBowman]

ASWJ again, we'd rather perform such position/velocity/torque limit checks on the driver side, i.e. in the TechnosoftIpos YARP device. The cartesian controller is meant to clip user-space variables (i.e. cartesian positions/velocities/accelerations), much in the vein of RAPID speeddata objects.

TBH I don't know how to proceed, and I also feel like throwing previous work and future ideas as laid out in https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/156 or https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/161.

[PeterBowman]

TBH I don't know how to proceed

Per https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/159:

• Remove intrinsic joint velocity checks in BCC.
• Move software limit checks to the low-level driver implementation (https://github.com/roboticslab-uc3m/yarp-devices/issues/188 + "dirty" estimations in IPosDirect via delta_sigma/duration).

[jgvictores]

IMHO they are not mutually exclusive, and perhaps not even redundant. Some unorganized ideas:

• We definitely want checks as described in the above figure at the joint level (e.g. a DNN generating joint-space commands to positionDirect). Note that this is somewhat "online".
• Something like https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/156 is nice. See my new comment there.
• https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/159 is still good for Cartesian-space velocities.

[PeterBowman]

Following https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/84739d0de78e8b23bfe6727b11261c6212ff22d9...

• Move software limit checks to the low-level driver implementation (roboticslab-uc3m/yarp-devices#188 + "dirty" estimations in IPosDirect via delta_sigma/duration).

These software limits are hardcoded values found in some .ini configuration file. I misunderstood this, since we don't need to query/estimate the instantaneous velocities at all (see https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/161#issuecomment-493167169).

• Remove intrinsic joint velocity checks in BCC.

See explanation to previous point and linked comment. Those checks are performed remotely (BCC side) against the speed limits fetched once from the robot on device configuration. In short: transform user-space velocities to joint velocities and ensure that those limits are not exceeded prior to sending the movement command.

[PeterBowman]

For the joint velocity check, the streaming rate of new position data is needed.

Remove intrinsic joint velocity checks in BCC.

Also, I no longer care about checking velocities in movi since the robot-side implementation paradigm has changed, see https://github.com/roboticslab-uc3m/yarp-devices/issues/198 for context. There is no need to preset streaming command send intervals in order to estimate the next command and thus obtain (estimate) the constant (point-to-point) resulting velocity as seen by the PT-mode iPOS interpolator. The drive itself will take care - robot-side limits will be respected and no action should be taken on the user-app side.

I'm going to partially revert https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/99ab1aa21336eb8d0a1729c8091df25200df1794 because of that.

[PeterBowman]

This app sends stop requests (at least with spnav devices), which currently set the controller back to position mode. Another control mode preset would be required.

Done at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/41349ecf574ae10b55fc6f48573053428e805922.

[PeterBowman]

Testing w/ spnav:

yarpdev --device SpaceNavigator --period 5 --name /spacenavigator --ports "(mouse buttons)" --channels 8 --mouse 0 5 0 5 --buttons 6 7 0 1

yarpdev --device BasicCartesianControl --name /bcc --remote /teoSim/leftArm --ik st --from /usr/local/share/teo-configuration-files/contexts/kinematics/fixedTrunk-leftArm-fetch-kinematics.ini --local /bcc/client

streamingDeviceController --streamingDevice SpaceNavigator --remoteCartesian /bcc

streamingDeviceController --streamingDevice SpaceNavigator --remoteCartesian /bcc --movi --gain 0.01

streamingDeviceController --streamingDevice SpaceNavigator --remoteCartesian /bcc --movi --gain 0.01 --SpaceNavigator::fixedAxes "(rotx roty rotz)" --period 0.01

[PeterBowman]

Tested on the real robot with a SpaceNavigator device:

Remarks:

• Motion is rather fluid with a 20 ms refresh period, but sometimes still a bit clunky (clac clac clac).
• No response to user commands after the application is relaunched. Reminds me of https://github.com/roboticslab-uc3m/yarp-devices/issues/198, probably requires low-level CAN maintenance work (again).

[PeterBowman]

I noticed that the position reference used for computing the relative distance between consecutive points when commanding via spacenav should not rely on online encoder data. Instead, I decided to operate on closed loop in that a "virtual" cartesian pose reference is maintained along the whole path, using the outcome of stat on device configuration as starting point: https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/c0f8460ff992c1f5ef14bc9ad69bda28a5bf10de. The open loop control derived in a constant drift of the robot arm towards the ground, regardless of the direction of the commanded vector.

[PeterBowman]

Testing w/ leap:

yarpdev --device LeapMotionSensor --period 10 --name /leap

yarpdev --device BasicCartesianControl --name /bcc --remote /teoSim/leftArm --ik st --from /usr/local/share/teo-configuration-files/contexts/kinematics/fixedTrunk-leftArm-fetch-kinematics.ini --local /bcc/client

streamingDeviceController --streamingDevice LeapMotionSensor --remoteCartesian /bcc --LeapMotionSensor::leapFrameRPY "(180 0 -90)" --LeapMotionSensor::fixedAxes "(rotx roty rotz)" --period 0.01 --scaling 2

streamingDeviceController --streamingDevice LeapMotionSensor --remoteCartesian /bcc --LeapMotionSensor::leapFrameRPY "(180 0 -90)" --LeapMotionSensor::fixedAxes "(rotx roty rotz)" --scaling 1 --movi

[PeterBowman]

Tested on the real robot with a Leap Motion device:

Sometimes, the last joints seem unable to keep up with the overall movement of the arm, perhaps due to the intrinsic speed limitation (10 deg/s).

[PeterBowman]

Also, I no longer care about checking velocities in movi since the robot-side implementation paradigm has changed, see roboticslab-uc3m/yarp-devices#198 for context. There is no need to preset streaming command send intervals in order to estimate the next command and thus obtain (estimate) the constant (point-to-point) resulting velocity as seen by the PT-mode iPOS interpolator. The drive itself will take care - robot-side limits will be respected and no action should be taken on the user-app side.

I'm going to partially revert 99ab1aa because of that.

Ran into https://github.com/roboticslab-uc3m/yarp-devices/issues/198#issuecomment-494778010, which derived in https://github.com/roboticslab-uc3m/yarp-devices/issues/198#issuecomment-495322968 - PT mode has been implemented again, command send intervales are indeed preset by the BCC, and velocity checks in movi have been ultimately restored at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/67106dba7fbb2c4d26dafe8f4a979fadc14e12c0.

Currently at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/e11d3f2541a42c778f467830683f0d6b68ede074, tests on the real robot look promising, further low-level improvements will be treated at https://github.com/roboticslab-uc3m/yarp-devices/issues/198 (and https://github.com/roboticslab-uc3m/yarp-devices/issues/208).

[PeterBowman]

velocity checks in movi have been ultimately restored at 67106db

ATOW:

• the robot board clips incoming targets if the distance travelled is too far away considering preconfigured velocity limits (https://github.com/roboticslab-uc3m/yarp-devices/commit/7cac9f4752593a23c2c8db04a272fd25b9189a93)
• BCC avoids sending a command at all, so the previous point doesn't apply anyway in our current cartesian-to-joint-space control scheme

[PeterBowman]

Done at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/21e7cb3841d4e3215e358e32da0ca384051f3807.

[PeterBowman]

More follow-ups linked to https://github.com/roboticslab-uc3m/yarp-devices/issues/198: velocity checks have been removed (again) at https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/ec013e3566cf70a921bc2abc97ced566b52f685d. Also, BCC does not care anymore about fixing a streaming period that would comply with PT/PVT mode thanks to a downsampling technique on the robot side that removes this burden altogether.