tud-phi / ros2-elastica Goto Github PK
View Code? Open in Web Editor NEWROS2 package implementing Elastica
ROS2 package implementing Elastica
I assume that files such as this one should not be in the repo:
After looking at your current implementation and looking through Elasticas tutorials and code, I realised that we need to change the actuation model:
We consider a soft robotic arm actuated pneumatically. Pneumatic actuation allows us to apply a constant torque over a specified sub-length of the arm (also called a segment).
We should implement this analogy to pneumatic actuation also in simulation. I think the easiest strategy would be to connect multiple rods with fixed joints. Every rod represents aa segment with one pneumatic actuation model. This means, that we should apply a uniform torque along the rod. The control input in this case should be a sequence of torques for each segment.
Please find out into which direction the UniformTorque on a rod is applied to. Is this in the world coordinate frame or the local coordinate frame of the rod? If we cannot adapt the direction of control online, we should add a UniformTorque into each cardinal direction, so that we can scale each cartesian torque component during the simulation
Accordingly, the current ControlInput msg can be kept, but should be renamed to MuscleTorques msg with fields direction, angular_frequency, wave_number, phase_shift, ramp_up_time and spline_control_points as documented here:
https://github.com/GazzolaLab/PyElastica/blob/master/elastica/_elastica_numpy/_external_forces.py#L200
In my opinion, using global variables is bad practice as they make the code-flow very hard to understand and can easily lead to bugs. Please replace them using class objects are similar strategies.
To me the naming of some of the files and classes does not make sense yet. Could you please revise this?
One example would be the filename of ros2_elastica_torque_force_params.py
and the class name of MinimalPublisherSubscriberForces
. The names should be easily understandable and follow some patterns and conventions
Please think about which parameters should be initialized as ROS parameters and put into a launch file.
For now, I am mainly thinking of the timer frequency, which is currently hard-coded: https://github.com/tud-cor-sr/ros2-elastica/blob/testing_versions/elastica_sim_control/elastica_sim_control/elastica_publisher_subscriber.py#L89
Please use the improved PCC state parametrization as introduced here:
https://ieeexplore.ieee.org/document/8961972
Please be aware that the forward transformation matrix in (14) has some wrong signs. You can find the correct version in the screenshot in the attachment.
I would also suggest to add the "Piecewise Constant Elongation" (e.g. the delta L
) as an additional configuration variable.
ruf
I am not convinced of fully relying on the rotation between the base and the tip to estimate the configuration of the segment. This strategy, can for example not capture any elongation of the segment. I would prefer to implement it analogue to what we are currently doing experimentally in our robot, which also takes into account the translation between the base and the tip.
You can use the proposed strategy in Equation (17) of the improved state parametrization paper by Cosimo. Please make sure to switch the sign of the division analogue to the blue marking in the attached screenshot.
I am creating this issue for future reference. Please implement first a soft robotic arm and a sphere into the simulation as already discussed previously.
Afterwards, I think the next step is to add some kinematics. This will be quite a bit more mathematical, than all the coding in the first part of the internship :) I am thinking of the following structure:
elastica_kinematics
RodStatesToPCC
with the RodsState.msg
as an input and the PCC kinematic states as an output. We aim to use the following PCC convention:Della Santina, Cosimo, Antonio Bicchi, and Daniela Rus. "On an improved state parametrization for soft robots with piecewise constant curvature and its use in model based control." IEEE Robotics and Automation Letters 5.2 (2020): 1001-1008.
The next components should (hopefully) be applicable for any soft robotic arm, not only for rods simulated within the elastica
simulator. Thus, we should probably put them into a separate repo with the name ros2-pcc_kinematics
scripts/derive_jacobian.py
, which computes the Jacobian symbolically using sympy and afterwards saves the result as a compiled C++ library. Then, our ROS nodes can access this C++ library for fast usage at run-time. We should have a Jacobian available at the minimum for every tip of each segment. Ideally, we can also evaluate the Jacobian within each segment as a function of a running variable s
with minimum 0 (e.g. the base of the segment) and 1 as the maximum (e.g. the tip of the segment).ForwardKinematics
node, which subscribes to a topic with PCC configuration states and publishes the position and orientation of the tip of each segment.Please try to avoid copying the elastica code and instead try to find another way for the implementation such as class inheritance etc
https://github.com/tud-cor-sr/ros2-elastica/blob/main/elastica_sim_control/elastica_sim_control/muscle_torques_with_bspline_numba.py
https://github.com/tud-cor-sr/ros2-elastica/blob/main/elastica_sim_control/elastica_sim_control/muscle_torques_with_bspline_numpy.py
I see that you are subscribing to some topics: https://github.com/tud-cor-sr/ros2-elastica/blob/testing_versions/elastica_sim_control/elastica_sim_control/elastica_publisher_subscriber.py#L270
However, it does not seem like you are doing anything with the received messages except printing them. I am missing the subscription to the control inputs and the forwarding to the elastica simulation.
I think it might make sense to put the simulation variables into a dictionary, so that the passing of these variables into functions and methods gets easier and if something in the simulator backend changes, you can easily add more variables to the dict.
I find the structure of the RodState.msg
way too inflexible. I propose the following structure:
In the RodStates.msg
, you report the state of all rods (e.g. pneumatically actuated segments). Accordingly, the message is structured like:
std_msgs/Header header
# number of rods
int num_rods
# all the rod states
RodState[] rod_states
Then, we have another message type RodState.msg
, where we report the state of all elements within that rod (so for example within the order of 51 elements as currently configured in the code):
std_msgs/Header header
# number of element for the rod
int num_elements
# the poses of all elements in the Cosserat rod
geometry_msgs/PoseStamped[] poses
It's always the better choice to use a standard message format such as PoseStamped
whenever possible. Also, this gives us the flexible to use however many rods we want to use
This issue shall describe the approach to implement some simple (model-based) controllers for the simulated soft robotic arm. Obviously, first #25 needs to be completed.
To get the soft robot moving, it makes sense to implement some simple PID / PD controller in configuration-space. A PD controller for set-point regulation would look like this:
Then, we should consider some simple model-based controllers. In particular interesting for set-point regulation would be:
\tau = Kq + G(q) + K_\mathrm{p} (\bar{q}-q) - K_\mathrm{d} \dot{q}
where the gains of the feedback component can be reduced or even set to zero.
This will require the identification of the elasticity matrix K
and the gravitational term G(q)
. While we can set the gravity to zero for the start, we can identify the (diagonal) constant matrix K
with the following strategy:
tau
to the system.K
with K=tau/q
K
with least-squaresPlease replace all print statements with ros logger messages (either info, warning, error or debug):
self.get_logger().info("Your message")
I do not understand why you publish the control input message again within the same node. Our controller, should go into a different node and that one should publish suitable control commands as ControlInput messages:
What is the meaning of the normal_director
? is it the direction of the local z-axis
in the global reference frame? If it is, then we should probably call it differently. Is director
even a word in the dictionary?
Please use a more intuitive name for the publisher and subscriber class attributes:
https://github.com/tud-cor-sr/ros2-elastica/blob/main/elastica_sim_control/elastica_sim_control/elastica_publisher_subscriber.py#L41
I think I found a bug in the continuum_flagella_ros2.py
file:
https://github.com/tud-cor-sr/ros2-elastica/blob/main/elastica_sim/elastica_sim/continuum_flagella_ros2.py#L150
I nowhere see an implementation of the function uniformtorques_mag_cal
. I think in the snake simulator, you commented this line.
A small formatting suggestion for the README:
Please rename rod_tip_orientation_X
to orientation_XX
. You can add a code comment that you always refer to the orientation of the tip of the rod:
https://github.com/tud-cor-sr/ros2-elastica/blob/main/elastica_sim/elastica_sim/continuum_flagella_ros2.py#L42
https://github.com/tud-cor-sr/ros2-elastica/blob/main/elastica_sim/elastica_sim/continuum_flagella_ros2.py#L187
This is more in line with the common nomenclature
I agree that it makes sense to share this code between different examples and simulation configurations. The other alternative to importing this class would be to add this class as a base-class to the simulator examples and then accordingly just call self.publish() to publish any new data. This would also make it easier to access the simulator attributes from the ROS publisher and subscriber as they should be able to be accessed with a simple self.attribute. So I think this option might be even better. What do you think?
To be more concrete, the FlagellaSimulator
class definition would for example now look like:
class FlagellaSimulator(BaseSystemCollection, Constraints, Forcing, CallBacks, ElasticaRos2PublisherSubscriber):
pass
Please rename the package elastica_sim_control
to elastica_sim
as it should only handle the simulation. The controller will be placed in a different package (but in the same repo).
Currently, you are only subscribing and publishing scalars or arrays of type Float64: https://github.com/tud-cor-sr/ros2-elastica/blob/testing_versions/elastica_sim_control/elastica_sim_control/elastica_publisher_subscriber.py
Our goal is to make the simulator plug-and-play with ROS packages we develop also for lab experiments, but also with other ROS packages developed by the community.
Please implement more standard control, geometry and sensor ROS messages in addition to the current publishing of the raw float messages. Examples for this are PoseArray.msg, AcecelStamped.msg, Vector3.msg etc
https://github.com/ros2/common_interfaces/tree/master/geometry_msgs/msg
For coordinate transformations, tf2 might be quite interesting: http://wiki.ros.org/tf2
I am wondering if the multiprocessing with one process each for simulation and ROS node is really necessary? I understand that the reason that you use the multiprocessing.Array is to share information between both processes?
Would it not be possible to run both the ROS node and the simulator within the same process? In the ContinuumFlagellaCallBack
class you would initiate the publishing of the current state of the robot (in the same process as the simulator) and then obviously with time_stepping
we would communicate the new control input which we have received from the ROS subscriber
I just want to make sure that the implemented solution is not more complicated than necessary, before we adapt to code to more environments and simulations.
Please populate the package.xml files:
https://github.com/tud-cor-sr/ros2-elastica/blob/testing_versions/src/ros2_elastica_pack/package.xml
Please do not just publish the orientation of the tip, but of all nodes:
https://github.com/tud-cor-sr/ros2-elastica/blob/main/elastica_sim_control/elastica_sim_control/continuum_flagella_ros2.py#L178
This will be relevant feedback for our controllers
The repository should not contain any install
, build
, log
files or folders.
It should have a similar to: https://github.com/tud-cor-sr/ros2-vtem_control or https://github.com/tud-cor-sr/ros2-mocap_optitrack
E.g. the folder structure should look like
First layer: elastica_{PACKAGE_NAME}
Second layer: package.xml
and src
, include
, launch
etc. folders of package
Please add a .gitignore
and add files and folders we do not want in the repo such as __pycache__
Please integrate the simulator into our Dockerfile: https://github.com/tud-cor-sr/sr-ros2-bundles
The repo can be added to the overlay.repo
file so that it is automatically downloaded. You can develop in your own branch and then create a PR at the end.
Important: The elastica dependencies and the repo should only be included in the Docker image, if an argument is set to true in docker-compose
. E.g. we should also be able to build docker images easily without the Elastica simulator
I do not see this variable used anywhere in the code
Rename no_of_segments
to num_segments
and add variable (currently set to 6) as a ROS2 variable
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.