Offline position-direct implementation via PT/PVT #245
Comments
PeterBowman
changed the title
Tiny grain of salt, regarding options we've contemplated in the past (I like the second one slightly better):
PS: There was a third option, via |
|
Current implementation looks promising, has been successfully tested via examplePositionDirect app. Config:
Workflow:
Tested with both PT and PVT submodes, interpolation time period ranging from 5 to 50 milliseconds, on a single node. I wonder whether bursting setpoints to six nodes at once would saturate the CAN bus. See: |
|
As mentioned above, the linear interpolation submodes are interfaced via
|
|
Velocity setpoints in the PVT submode are currently obtained via arithmetic mean of the mean velocities between three consecutive points. That is, the mean velocity is obtained between the current and the previous setpoints in the queue, same for the next and the current setpoints, and then both results are weighed (same period, so this is as simple (A+B)/2).
I wonder if this solution could yield better results in a cubic interpolation (regarding position data). Surely it is less convolute, must check its "jerkiness". |
This MATLAB script performs cubic interpolation on a randomly generated curve: splinetest.zip. It also plots y (position) against x (time), therefore slopes are instantaneous velocities. Three different approaches regarding slope values are depicted in the following figure. Cyan means zero slope on boundaries and blue points, red line represents the differential approach (mean slope between current and previous point), and black line corresponds to the currently implemented arithmetic mean approach (weighed result of previous and next slopes):
Out of sheer curiosity, MATLAB's
I definitely like the implemented approach. |
I'm considering this option (again), mainly because of:
|
|
Considering that from path-planning now we can have a nice big Bottle that encapsulates a Trajectory:
|
|
Note: problems reported by ControlboardWrapper such as Error while trying to command a streaming position direct message on all joints are caused by hardware faults and elevated through the current control mode check. See |
Note that this is in fact a |
|
After a long chat with @jgvictores in the aftermath of #254 (comment), we've summarized the main use cases for the variety of
I am going to propose an asynchronous PT/PVT implementation in clear contrast to CSP, which is probably going to stay the way it is now. Going async means that the drive will not expect new setpoints at a fixed, preconfigured rate (we used to work with a period of 50 milliseconds). Instead, the delay between successive points will be computed on-the-fly, i.e. as soon as TechnosoftIpos receives point n+1, it substracts current time from the time point n was received and sets the "T" component accordingly. In short: if n+1 arrived 47 ms later than n, instruct the drive to perform n+1 in 47 ms. In consequence, new setpoints may be sent in arbitrary time periods. This solution aims to decouple external control applications from the internal point processing rates in synchronous drive modes. In conclusion, expected use cases are:
Once implemented and tested, these guidelines should land in teo-developer-manual. As a side note, implementations could take advantage of some filtering. See the iCub's collection of utilities. |
In fact, the sync port can be easily emulated with |
|
Mostly implemented at 2308f03, needs testing. Main remarks regarding PT/PVT submodes:
See #198 (comment) regarding buffer sizes. Default buffer low limit has been set to 4 points. Workflow:
In order to implement asynchronous PT/PVT, the internal queue stores setpoint-timestamp pairs. The exact time at which each point is placed in the queue is captured in order to obtain the difference between successive points. For instance, if point n1 was sent by the user at timestamp t1 and n2 at t2, then the result of To be considered (TO-DO list):
|
Logs: Sequence:
I'm quite positive that the integrity counter feedback can be used to implement a simple yet effective handshake. PS newly generated logs of the full trajectory (old implementation at 8dbd67f): old-ip-21022021.txt. |
|
For this very reason, I am not considering this option anymore:
PS my bad, TPDO2 is mapped to MER/DER: yarp-devices/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp Lines 129 to 130 in 8a2910d I don't think there is an inhibit time setting for EMCY messages, so they are supposed to be triggered as soon as an emergency condition arises. From CiA 301 v4.2.0, 7.2.7.1, Emergency object usage:
Anyway, I'd rather keep sending points in batches (as opposed to mimicking a handshake via individual requests and integrity counter checks) to avoid the saturation of the CAN network and timing issues. |
|
While synchronous IP (interpolated position) works fine, asynchronous needs still more work:
Quick test: I have sent 2000 synchronous PT points that theoretically should have been executed in 20 seconds (period equal to 10 ms). I measured the time elapsed between buffer full and buffer empty signals and got only 19 seconds. There is a difference of 0.5 seconds if I supply half the number of points (period 20 ms), and 2 seconds if I double that amount (period 5 ms). It seems that the interpolation substracts like 0.5 ms from each setpoint. |
|
ASWJ there is a bunch of use cases that involve us implementing
Definitions:
Methods:
|
|
I've been doing my first tests, launching the trajectory of the issue #254 (comment) -> test_elbow.zip and modifying your example examplePositionDirect.cpp to execute it in Synchronous PT/PVT (default in pt), reading the csv file. My program would be the following:#include <yarp/os/Network.h>
#include <yarp/os/Property.h>
#include <yarp/os/ResourceFinder.h>
#include <yarp/os/Time.h>
#include <yarp/dev/IControlMode.h>
#include <yarp/dev/IEncoders.h>
#include <yarp/dev/IPositionControl.h>
#include <yarp/dev/IPositionDirect.h>
#include <yarp/dev/IRemoteVariables.h>
#include <yarp/dev/PolyDriver.h>
#include <ColorDebug.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <iomanip>
#define DEFAULT_ROBOT "/teo"
#define DEFAULT_JOINT_ID 3 // elbow
#define DEFAULT_POSD_PERIOD_MS 50
#define DEFAULT_IP_MODE "pt"
int main(int argc, char *argv[])
{
yarp::os::ResourceFinder rf;
rf.configure(argc, argv);
std::string robot = rf.check("robot",yarp::os::Value(DEFAULT_ROBOT),"name of robot to be used").asString();
int jointId = rf.check("joint", yarp::os::Value(DEFAULT_JOINT_ID), "joint number to test").asInt32();
int period = rf.check("period", yarp::os::Value(DEFAULT_POSD_PERIOD_MS), "posd command period [ms]").asInt32();
bool batch = rf.check("batch", "stream interpolation data in batches");
std::string ipMode = rf.check("ipMode", yarp::os::Value(DEFAULT_IP_MODE), "linear interpolation mode [pt|pvt]").asString();
if (period <= 0)
{
CD_ERROR("Illegal period: %d.\n", period);
return false;
}
if (ipMode != "pt" && ipMode != "pvt")
{
CD_ERROR("Illegal ipMode: %s.\n", ipMode.c_str());
return false;
}
yarp::os::Network yarp;
if (!yarp::os::Network::checkNetwork())
{
CD_ERROR("Please start a yarp name server first.\n");
return 1;
}
yarp::dev::IControlMode * ramode;
yarp::dev::IEncoders * raencs;
yarp::dev::IPositionControl * rapos;
yarp::dev::IPositionDirect * raposd;
yarp::dev::IRemoteVariables * ravar;
yarp::os::Property raoptions;
raoptions.put("device","remote_controlboard");
raoptions.put("remote",robot+"/rightArm");
raoptions.put("local","/test"+robot+"/rightArm");
yarp::dev::PolyDriver radev(raoptions);
radev.open(raoptions);
if(!radev.isValid()) {
CD_ERROR("robot rightArm device not available.\n");
radev.close();
yarp::os::Network::fini();
return false;
}
bool ok = true;
ok &= radev.view(ramode);
ok &= radev.view(raencs);
ok &= radev.view(rapos);
ok &= radev.view(raposd);
ok &= !batch || radev.view(ravar);
if (!radev.isValid())
{
CD_ERROR("Remote device not available.\n");
return 1;
}
if (!ok)
{
CD_ERROR("Problems acquiring robot interfaces.\n");
return 1;
}
if (batch)
{
yarp::os::Property dict {{"enable", yarp::os::Value(true)},
{"mode", yarp::os::Value(ipMode)},
{"periodMs", yarp::os::Value(period)}};
yarp::os::Value v;
v.asList()->addString("linInterp");
v.asList()->addList().fromString(dict.toString());
if (!ravar->setRemoteVariable("all", {v}))
{
CD_ERROR("Unable to set linear interpolation mode.\n");
return 1;
}
}
if (!ramode->setControlMode(jointId, VOCAB_CM_POSITION_DIRECT))
{
CD_ERROR("Problems setting position control: POSITION_DIRECT.\n");
return 1;
}
std::ifstream lectura;
std::vector<double> pose;
double value;
lectura.open("../test_elbow.csv",std::ios::in);
for (std::string linea; std::getline(lectura, linea); )
{
std::stringstream registro(linea);
for (std::string dato; std::getline(registro, dato, ','); )
{
pose.push_back( std::stod(dato));
}
CD_INFO_NO_HEADER("sending position: %f to joint: %d period: %d ms\n", pose[3], jointId, period);
raposd->setPosition(jointId, pose[3]);
if (!batch)
{
yarp::os::Time::delay(period * 0.001);
}
pose.clear();
}
CD_INFO_NO_HEADER("end\n");
return 0;
}Terminal 1: Terminal 2 of launchCanBus (from the connection between yarp ports of the application with the launchCanBus):The result is that there is a loss of control when the arm returns. I've repeated the experiment executing it as before without Video in PT mode: https://youtu.be/yR4z7nSV7TE |
|
Run the experiment again, uncommenting this line. exactly, the first jump occurs on line 1686: [debug] IPositionDirectRawImpl.cpp:13 setPositionRaw(): (0, -83.383257)
[debug] IPositionDirectRawImpl.cpp:13 setPositionRaw(): (0, -61.902524)I've also seen this jump (although it doesn't arrive here) [debug] IPositionDirectRawImpl.cpp:13 setPositionRaw(): (0, -45.270002)
[debug] IPositionDirectRawImpl.cpp:13 setPositionRaw(): (0, -12.867257) |
|
Applying a delay in the sending of 25ms (half of the execution period of 50ms by default) between one point and the next, the result is that it performs the execution of the trajectory successfully without any failure. |
|
Added a delay between sending setpoints half the execution period: if (!batch)
{
yarp::os::Time::delay(period * 0.001);
}
else
{
yarp::os::Time::delay(period * 0.0005); // -> period / 2
}I've been reducing the period little by little until reaching 5 ms. It looks nice ^^! : |
|
I'm going to comment on some of the experiments that I've carried out to test the PT mode, executing an offline trajectory obtained from a CSV file. In this case, it's a trajectory captured from the mocap system and later processed through the teo blender project. Then, I've created a program that allows reading the csv file and sending the commands in PT / PVT mode to the robot or CSP to the simulator, following the example of the tests carried out previously.
I've tried to run the same csv both in the simulator (in CSP mode by default without the @PeterBowman suggested to me to visualize the current value of The result of the values printed by terminal can be reviewed here. In conclusion, not all drivers execute the trajectory in unison or end at the same time. |
|
Not accounting for trunk joints, time measurements for all upper body joints look as follows:
The theoretical duration of this trajectory is 123 seconds. However, motion was completed sooner that expected and also inconsistently across all joints, varying from 0.08 seconds (quite close) to 4.16 seconds (worse case). |
|
I've replicated the drift bug with only ID26 up and running on the network, no SYNC (using default TPDO3 with 30 ms inhibit time), 2000 PT points with period 10 ms (from 50º to -50º at 5º/s): dump.txt. Still getting 19 seconds instead of 20. Let's take a look at these two consecutive PT points (logged by Bytes 4 and 5 should read 0x000a, i.e. 10 samples (time component). As soon as the target position goes negative, the two's complement system wreaks havoc on the most significant bytes coming next, i.e. the 0x000a is turned into 0x0009 (9 samples). That's why the trajectory ended prematurely. Fixed at a881f95. |
Along the lines of #259, it should not be necessary to apply this hardcoded delay if the remote_controlboard device is configured with |
|
Commit e8d69d3 increases the threshold for motion start trigger on the internal queue size so that batches in async pt/pvt are as big as possible (not accounting for occasional timer drifts). Now, despite a max HW buffer size of 9 and 7 points for PT and PVT submodes, respectively, the internal queue needs to allocate 10 and 9 points prior to commanding motion start (one extra point in PVT so that the mean velocity can be extrapolated). I have successfully tested sync/async pt/pvt with base command PS sending a batch of 5000 PT sync points did not cause CBW to print a warning (using |
|
Re-launched the same trajectory in PT mode with the new changes. Executed with a period of 20ms/setpoint. |
Stemming from #222, which drops legacy PT/PVT interpolation modes in favor of CSP for online commands (e.g. joystick teleoperation) since the single-element buffer solution was never a good choice anyway, I want to focus on offline usage to actually take advantage of said buffer.
Issues:
Setpoints need to be stored in memory prior to populating the buffer(s): keep in mind it will be pretty easy to overflow the 500-element PCan TX buffer despite having a lower PT/PVT buffer limit (285/222 setpoints, respectively). Those limits do not scale well since there are usually several nodes on the same bus (285 * 6 = ?).
In the same manner, either the CanBusPeak implementation or CAN RW threads of CanBusControlboard should internally manage a queue of messages that exceeds the hardware-limit (500 CAN frames). Currently, it is not allowed (via software) to register more messages into a full queue.
Possible usage:
IRemoteVariables(Implement cyclic synchronous position (CSP) mode #222 (comment)).IPositionDirect::setPositionssuccessively in order to save desired setpoints into memory.See #198 (comment) regarding PT/PVT buffer size and how to increase it. Default is 9 PT or 7 PVT points.
The text was updated successfully, but these errors were encountered: