Beautiful. Since we’re currently just using a 256×256 view port in pybullet, this is quite a bit more advanced than required though. Learning game engines can also take a while. It took me about a month to learn Unity3d, with intermediate C# experience. Unreal Engine uses C++, so it’s a bit less accessible to beginners.
Continuing from our early notes on SLAM algorithms (Simultaneous Localisation and Mapping), and the similar but not as map-making, DSO algorithm, I came across a good project (“From cups to consciousness“) and article that reminded me that mapping the environment or at least having some sense of depth, will be pretty crucial.
At the moment I’ve just got to the point of thinking to train a CNN on simulation data, and so there should also be some positioning of the robot as a model in it’s own virtual world. So it’s probably best to reexamine what’s already out there. Visual odometry. Optical Flow.
I found a good paper summarizing 2019 options. The author’s github has some interesting scripts that might be useful. It reminds me that I should probably be using ROS and gazebo, to some extent. The conclusion was roughly that Google Cartographer or GMapping (Open SLAM) are generally beating some other ones, Karto, Hector. Seems like SLAM code is all a few years old. Google Cartographer had some support for ‘lifelong mapping‘, which sounded interesting. The robot goes around updating its map, a bit. It reminds me I saw ‘PonderNet‘ today, fresh from DeepMind, which from a quick look is, more or less, about like scaling your workload down to your input size.
Anyway, we are mostly interested in Monocular SLAM. So none of this applies, probably. I’m mostly interested at the moment, in using some prefab scenes like the AI2Thor environment in the Cups-RL example, and making some sort of SLAM in simulation.
Also interesting is RATSLAM and the recent update: LatentSLAM – The authors of this site, The Smart Robot, got my attention because of the CCNs. Cortical column networks.
“A common shortcoming of RatSLAM is its sensitivity to perceptual aliasing, in part due to the reliance on an engineered visual processing pipeline. We aim to reduce the effects of perceptual aliasing by replacing the perception module by a learned dynamics model. We create a generative model that is able to encode sensory observations into a latent code that can be used as a replacement to the visual input of the RatSLAM system”
Interesting, “The robot performed 1,143 delivery tasks to 11 different locations with only one delivery failure (from which it recovered), traveled a total distance of more than 40 km over 37 hours of active operation, and recharged autonomously a total of 23 times.“
I think DSO might be a good option, or the closed loop, LDSO, look like the most straight-forward, maybe.
After a weekend away with a computer vision professional, I found out about COLMAP, a structure from movement suite.
I saw a few more recent projects too, e.g. NeuralRecon, and
ooh, here’s a recent facebook one that sounds like it might work!
Consistent Depth … eh, their google colab is totally broken.
Anyhow, LDSO. Let’s try it.
In file included from /dmc/LDSO/include/internal/OptimizationBackend/AccumulatedTopHessian.h:10:0, from /dmc/LDSO/include/internal/OptimizationBackend/EnergyFunctional.h:9, from /dmc/LDSO/include/frontend/FeatureMatcher.h:10, from /dmc/LDSO/include/frontend/FullSystem.h:18, from /dmc/LDSO/src/Map.cc:4: /dmc/LDSO/include/internal/OptimizationBackend/MatrixAccumulators.h:8:10: fatal error: SSE2NEON.h: No such file or directory #include "SSE2NEON.h" ^~~~ compilation terminated. src/CMakeFiles/ldso.dir/build.make:182: recipe for target 'src/CMakeFiles/ldso.dir/Map.cc.o' failed make[2]: *** [src/CMakeFiles/ldso.dir/Map.cc.o] Error 1 make[2]: *** Waiting for unfinished jobs…. CMakeFiles/Makefile2:85: recipe for target 'src/CMakeFiles/ldso.dir/all' failed make[1]: *** [src/CMakeFiles/ldso.dir/all] Error 2 Makefile:83: recipe for target 'all' failed make: *** [all] Error 2
Ok maybe not.
There’s a paper here reviewing ORBSLAM3 and LDSO, and they encounter lots of issues. But it’s a good paper for an overview of how the algorithms work. We want a point cloud so we can find the closest points, and not walk into them.
Calibration is an issue, rolling shutter cameras are an issue, IMU data can’t be synced meaningfully, it’s a bit of a mess, really.
Also, reports that ORB-SLAM2 was only getting 5 fps on a raspberry pi, I got smart, and looked for something specifically for the jetson. I found a depth CNN for monocular vision on the forum, amazing.
Ok so after much fussing about, I found just what we need. I had an old copy of jetson-containers, and the slam code was added just 6 months ago. I might want to try the noetic one (ROS2) instead of ROS, good old ROS.
git clone https://github.com/dusty-nv/jetson-containers.git
cd jetson-containers
chicken@jetson:~/jetson-containers$ ./scripts/docker_build_ros.sh --distro melodic --with-slam
Successfully built 2eb4d9c158b0
Successfully tagged ros:melodic-ros-base-l4t-r32.5.0
chicken@jetson:~/jetson-containers$ ./scripts/docker_test_ros.sh melodic
reading L4T version from /etc/nv_tegra_release
L4T BSP Version: L4T R32.5.0
l4t-base image: nvcr.io/nvidia/l4t-base:r32.5.0
testing container ros:melodic-ros-base-l4t-r32.5.0 => ros_version
xhost: unable to open display ""
xauth: file /tmp/.docker.xauth does not exist
sourcing /opt/ros/melodic/setup.bash
ROS_ROOT /opt/ros/melodic/share/ros
ROS_DISTRO melodic
getting ROS version -
melodic
done testing container ros:melodic-ros-base-l4t-r32.5.0 => ros_version
Well other than the X display, looking good.
Maybe I should just plug in a monitor. Ideally I wouldn’t have to, though. I used GStreamer the other time. Maybe we do that again.
This looks good too… https://github.com/dusty-nv/ros_deep_learning but let’s stay focused. I’m also thinking maybe we upgrade early, to noetic. Ugh it looks like a whole new bunch of build tools and things to relearn. I’m sure it’s amazing. Let’s do ROS1, for now.
Let’s try build that FCNN one again.
CMake Error at tx2_fcnn_node/Thirdparty/fcrn-inference/CMakeLists.txt:121 (find_package):
By not providing "FindOpenCV.cmake" in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by "OpenCV", but
CMake did not find one.
Could not find a package configuration file provided by "OpenCV" (requested
version 3.0.0) with any of the following names:
OpenCVConfig.cmake
opencv-config.cmake
Add the installation prefix of "OpenCV" to CMAKE_PREFIX_PATH or set
"OpenCV_DIR" to a directory containing one of the above files. If "OpenCV"
provides a separate development package or SDK, be sure it has been
installed.
-- Configuring incomplete, errors occurred!
Ok hold on…
Builds additional container with VSLAM packages,
including ORBSLAM2, RTABMAP, ZED, and Realsense.
This only applies to foxy and galactic and implies
--with-pytorch as these containers use PyTorch."
Ok that hangs when it starts building the slam bits. Luckily, someone’s raised the bug, and though it’s not fixed, Dusty does have a docker already compiled.
So, after some digging, I think we can solve the X problem (i.e. where are we going to see this alleged SLAMming occur?) with an RTSP server. Previously I used GStreamer to send RTP over UDP. But this makes more sense, to run a server on the Jetson. There’s a plugin for GStreamer, so I’m trying to get the ‘dev’ version, so I can compile the test-launch.c program.
apt-get install libgstrtspserver-1.0-dev
Reading package lists... Done
Building dependency tree
Reading state information... Done
libgstrtspserver-1.0-dev is already the newest version (1.14.5-0ubuntu1~18.04.1).
ok... git clone https://github.com/GStreamer/gst-rtsp-server.git
root@jetson:/opt/gst-rtsp-server/examples# gcc test-launch.c -o test-launch $(pkg-config --cflags --libs gstreamer-1.0 gstreamer-rtsp-server-1.0)
test-launch.c: In function ‘main’:
test-launch.c:77:3: warning: implicit declaration of function ‘gst_rtsp_media_factory_set_enable_rtcp’; did you mean ‘gst_rtsp_media_factory_set_latency’? [-Wimplicit-function-declaration]
gst_rtsp_media_factory_set_enable_rtcp (factory, !disable_rtcp);
^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
gst_rtsp_media_factory_set_latency
/tmp/ccC1QgPA.o: In function `main':
test-launch.c:(.text+0x154): undefined reference to `gst_rtsp_media_factory_set_enable_rtcp'
collect2: error: ld returned 1 exit status
gst_rtsp_media_factory_set_enable_rtcp
Ok wait let’s reinstall gstreamer.
apt-get install libgstreamer1.0-dev libgstreamer-plugins-base1.0-dev libgstreamer-plugins-bad1.0-dev gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-doc gstreamer1.0-tools gstreamer1.0-x gstreamer1.0-alsa gstreamer1.0-gl gstreamer1.0-gtk3 gstreamer1.0-qt5 gstreamer1.0-pulseaudio
error...
Unpacking libgstreamer-plugins-bad1.0-dev:arm64 (1.14.5-0ubuntu1~18.04.1) ...
Errors were encountered while processing:
/tmp/apt-dpkg-install-Ec7eDq/62-libopencv-dev_3.2.0+dfsg-4ubuntu0.1_arm64.deb
E: Sub-process /usr/bin/dpkg returned an error code (1)
Ok then leave out that one...
apt --fix-broken install
and that fails on
Errors were encountered while processing:
/var/cache/apt/archives/libopencv-dev_3.2.0+dfsg-4ubuntu0.1_arm64.deb
It’s like a sign of being a good programmer, to solve this stuff. But damn. Every time. Suggestions continue, in the forums of those who came before. Let’s reload the docker.
Ok I took a break and got lucky. The test-launch.c code is different from what the admin had.
Let’s diff it and see what changed…
#define DEFAULT_DISABLE_RTCP FALSE
from
static gboolean disable_rtcp = DEFAULT_DISABLE_RTCP;
{"disable-rtcp", '\0', 0, G_OPTION_ARG_NONE, &disable_rtcp,
"Whether RTCP should be disabled (default false)", NULL},
from
gst_rtsp_media_factory_set_enable_rtcp (factory, !disable_rtcp);
so now this works (to compile).
gcc test.c -o test $(pkg-config --cflags --libs gstreamer-1.0 gstreamer-rtsp-server-1.0)
So apparently now I can run this in VLC… when I open
rtsp://<jetson-ip>:8554/test
Um is that meant to happen?…. Yes!
Ok next, we want to see SLAM stuff happening. So, ideally, a video feed of the desktop, or something like that.
So hereare the links I have open. Maybe I get back to them later. Need to get back to ORBSLAM2 first, and see where we’re at, and what we need. Not quite /dev/video0 to PC client. More like, ORBSLAM2 to dev/video0 to PC client. Or full screen desktop. One way or another.
libgstrtspserver-1.0-dev is already the newest version (1.14.5-0ubuntu1~18.04.1).
Today we have
E: Unable to locate package libgstrtspserver-1.0-dev E: Couldn't find any package by glob 'libgstrtspserver-1.0-dev' E: Couldn't find any package by regex 'libgstrtspserver-1.0-dev'
Did I maybe compile it outside of the docker? Hmm maybe. Why can’t I find it though? Let’s try the obvious… but also why does this take so long? Network is unreachable. Network is unreachable. Where have all the mirrors gone?
apt-get update
Ok so long story short, I made another docker file. to get gstreamer installed. It mostly required adding a key for the kitware apt repo.
Since 1.14, the use of libv4l2 has been disabled due to major bugs in the emulation layer. To enable usage of this library, set the environment variable GST_V4L2_USE_LIBV4L2=1
but it doesn’t want to work anyway. Ok RTSP is almost a dead end.
I might attach a CSI camera instead of V4L2 (USB camera) maybe. Seems less troublesome. But yeah let’s take a break. Let’s get back to depthnet and ROS2 and ORB-SLAM2, etc.
depthnet: error while loading shared libraries: /usr/lib/aarch64-linux-gnu/libnvinfer.so.8: file too short
Ok, let’s try ROS2.
(Sorry, this was supposed to be about SLAM, right?)
As a follow-up for this post…
I asked about mapping two argus (NVIDIA’s CSI camera driver) node topics, in order to fool their stereo_proc, on the github issues. No replies, cause they probably want to sell expensive stereo cameras, and I am asking how to do it with $15 Chinese cameras.
I looked at DustyNV’s Mono depth. Probably not going to work. It seems like you can get a good depth estimate for things in the scene, but everything around the edges reads as ‘close’. Not sure that’s practical enough for depth.
I looked at the NVIDIA DNN depth. Needs proper stereo cameras.
I looked at NVIDIA VPI Stereo Disparity pipeline It is the most promising yet, but the input either needs to come from calibrated cameras, or needs to be rectified on-the-fly using OpenCV. This seems like it might be possible in python, but it is not obvious yet how to do it in C++, which the rest of the code is in.
I attached two RPi 2.1 CSI cameras, from older projects. Deep dived into ISAAC_ROS suite. Left ROS2 alone for a bit because it is just getting in the way. The one camera sensor had fuzzy lines going across, horizontally, occasionally, and calibration results were poor, fuzzy. Decided I needed new cameras.
IMX-219 was used by the github author, and I even printed out half of the holder, to hold the cameras 8cm apart.
I tried calibration using the ROS2 cameracalibrator, which is a wrapper for a opencv call, after starting up the camera driver node, inside the isaac ros docker.
(Because of bug, also sometimes need to remove –ros-args –remap )
OpenCV was able to calibrate, via the ROS2 application, in both cases. So maybe I should just grab the outputs from that. We’ll do that again, now. But I think I need to print out a chessboard and just see how that goes first.
I couldn’t get more than a couple of matches using pictures of the chessboard on the screen, even with binary thresholding, in the author’s calibration notebooks.
Here’s what the NVIDIA VPI 1.2’s samples drew, for my chess boards:
Stereo DisparityConfidence Map
Camera calibration seems to be a serious problem, in the IOT camera world. I want something approximating depth, and it is turning out that there’s some math involved.
Learning about epipolar geometry was not something I planned to do for this.
But this is like a major showstopper, so either, I must rectify, in real time, or I must calibrate.
“The reason for the noisy result is that the VPI algorithm expects the rectified image pairs as input. Please do the rectification first and then feed the rectified images into the stereo disparity estimator.”
So can we use this info? The nvidia post references this code below as the solution, perhaps, within the context of the code below. Let’s run it on the chessboard?
I’ve been scouring for existing code to help with developing the gripper in simulation. I was looking for a way to implement ‘eye-in-hand’ visual servoing, and came across a good resource, created for a masters thesis, which shows a ‘robot vision’ window, and he compares depth sensing algorithms. My approach was going to be, essentially, segmentation, in order to detect and localise chickens and eggs, in the field of vision, and then just try get their shape into an X-Y coordinate position, and over a certain size, to initiate interaction.
This one uses an SDF model of a KUKA industrial 6 DOF robot with a two finger gripper, but that has specific rotational movement, that seems maybe different from a simpler robot arm. So it’s maybe a bit overkill, and I might just want to see his camera code.
Miranda’s gripper prototype isn’t a $50k KUKA industrial robot arm. It’s just v.0.1 and got an 11kg/cm MG945, some 5kg/cm MG5010s, and an 1.3kg/cm SG90, and a sucker contraption I found on DFRobot, that can suck eggs.
So, regarding the simulation,this will be on top of the robot, as its head.
So we need an URDF file. Or an SDF file. There’s a couple ways to go with this.
The other resource I’ve found that looks like just what I need, is ur5pybullet
Regarding the ‘visual servoing’, the state of the art appears to be QT-Opt, perhaps. Or maybe RCAN, built on top of it. But we’re not there just yet. Another project specifically uses pybullet. Some extra notes here, from Sergey Levine, and co., associated with most of these projects.
Another good one is Retina-GAN, where they convert both simulation and reality into a canonical format. I’ve also come across Dex-Net before, from UCB.
Everything is very complicated though.
I’ve managed to make an URDF that looks good enough to start with, though. I’ll put everything in a github. We want to put two servos on the ‘head’ for animatronic emotional aesthetics, but there’s a sucker contraption there for the egg, so I think this is good enough for simulation, for now, anyway. I just need to put a camera on its head, put some eggs in the scene, and maybe reward stable contact with the tip. Of course it’s going to be a lot of work.
We also want to add extra leg parts, but I don’t want to use 4 more motors on it.
So I’m playing around with some aluminium and timing belts and pulleys to get 8 leg parts on 4 motors. Something like this, with springs if we can find some.
So, simulator camera vision. I can enable the GUI. Turns out I just need to press ‘g’ to toggle.
Ok I’ve got the visuals now, but I shouldn’t be seeing that shadow
The camera is like 90 degrees off maybe. Could be an issue with the camera setup, or maybe the URDF setup? Ok…
Changing the initial camera vector fixed the view somewhat:
init_camera_vector = (0, 0, 1) # x-axis
Except that we’re looking backwards now.
init_camera_vector = (0, 0, -1) # x-axis
Ok well it’s correct now, but heh, hmm. Might need to translate the camera just a bit higher.
I found a cool free chicken obj file with Creative commons usage. And an egg.
Heh need to resize obj files. Collision physics is fun.
Ok I worked out how to move the camera a bit higher.
pos = list(pos)
pos[2] += 0.3
pos = tuple(pos)
Alright! Getting somewhere.
So, next, I add resized eggs and some chickens for good measure, to the scene.
Then we need to train it to stick its shnoz on the eggs.
Ok… gonna have to train this sucker now.
First, the table is falling from the sky, so I might need to stabilize it first. I also need to randomize the egg location a bit.
And I want to minimize the distance between the gripper attachment and the egg.
The smart way is probably to have it walk until some condition and then grasp, but in the spirit of letting the robot learn things by itself, I will probably ignore heuristics. If I do decide to use heuristics, it will probably be a finite state machine with ‘walking’ mode and ‘gripping’ mode. But we’ll come back to this when it’s necessary. Most of the time there won’t be any eggs in sight. So it will just need to walk around until it is sure there is an egg somewhere in sight.
And the end effector’s position should be something like the original camera position before we moved it up a bit, plus length of the end effector in the URDF (0.618). I ended up doing this:
The chicken bone robot prototype I made turned out pretty well, mostly because bones have kind of gone through millions of years of evolution to be as strong and light as possible. Sounds like an ideal limb material. It’s also really nice to work with. Would be nice to do some composites too, for molding and 3D printing etc. from egg shells and feathers (2.3 million tonnes of EU feather waste from slaughterhouses a year) and whatever else.
“Ecce” Robot pics from taken at the “making robots human ” exhibition in Stockholm Dan and I went to. The exhibition was kinda out of date British nationalist techno-utopian propaganda, but whatever, still got some cool hardware inspiration.
Going through the Bullet3 physics engine example code for ‘MotorDemo’. It’s a hexapod looking robot flexing its joints. The folder is bullet3/examples/DynamicControlDemo/
#ifndef MOTORDEMO_H
#define MOTORDEMO_H
class CommonExampleInterface* MotorControlCreateFunc(struct CommonExampleOptions& options);
#endif
C++ declares the function in .h files so that when compiling, all the .cpp files know about each other, without necessarily caring about the implementation details.
So this function is called ‘MotorControlCreateFunc’, which returns a pointer to a CommonExampleInterface, and takes a reference to CommonExampleOptions. Nice OOPy programming.
Let’s get into the CPP file.
Ok variables are:
time, cycle period, muscle strength, and an array of TestRig objects called rig
Constructor takes a pointer to a GUIHelperInterface. Destructor does no cleaning up.
They define an ‘initPhysics‘ and ‘exitPhysics‘. exitPhysics seems to have all the destructor code freeing up memory.
And three functions, spawnTestRig, setMotorTargets, and resetCamera.
TestRig consists of a world, collision shapes, bodies and joints.
Let’s look at the rest of the code, because most of the code takes place in TestRig.
So as mentioned before, initPhysics and exitPhysics set up and shut down the physics engine gui.
So this code will be heavily copy-pasted. In this case, it sets up the 3d world, makes a ground, and spawns two test rigs. (Spider things), with a small ‘start offset’ vector from each other. They are pushed onto the rigs stack variable.
setMotorTargets takes a time as a variable. The comment is
// set per-frame sinusoidal position targets using angular motor (hacky?)
So it iterates over the number of rigs, and then twice per NUM_LEGS, sets up target hinge variables, for the animation.
Per hinge, it gets the current angle, sets the target percentage as time divided by 1000, mod the cycle period, divided by the cycle period. and then sets the target angle as 0.5 * (1 + sin (2*PI*target percentage)). Ok…
Target Limit Angle is the hinge’s lower limit, plus the ( target angle multiplied by the ( upper limit – lower limit)). Ok…
Angle Error is the target limit angle minus the current angle.
DesiredAngularVelocity is angle error / time, which is passed to each hinge’s enableAngularMotor function.
I’d be lying if I said I understood entirely. But this is pretty much the basis of the program.
