tud-phi / ros2-elastica Goto Github PK

I assume that files such as this one should not be in the repo:

https://github.com/tud-cor-sr/ros2-elastica/blob/testing_versions/src/ros2_elastica_pack/ros2_elastica_pack/publisher_member_function.py

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.

https://github.com/tud-cor-sr/ros2-elastica/blob/testing_versions/src/ros2_elastica_pack/ros2_elastica_pack/examples/ContinuumFlagellaCase/continuum_flagella_ros2.py#L45

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

https://github.com/tud-cor-sr/ros2-elastica/blob/436afc30f7909b9397da65641e277c39e012fb61/elastica_kinematics/elastica_kinematics/elastica_rod_states_to_PCC.py#L58

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

https://github.com/tud-cor-sr/ros2-elastica/blob/436afc30f7909b9397da65641e277c39e012fb61/elastica_kinematics/elastica_kinematics/elastica_rod_states_to_PCC.py#L93

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:

• The package should be named elastica_kinematics
• We have these rod states now. These are way to high-dimensional though to use them for any kind of feedback control purposes. Additionally, we want to re-use as much of the learnings from literature as possible. Therefore, we should derive a standard Piecewise Constant Curvature (PCC) kinematic description of the soft robotic state. This means that we model every segment to have constant curvature, even if there are small deviations in the rod states of one segment. The node could have the name 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

• A main component of (inverse) kinematics is usually the Jacobian. You will need to derive this symbolically. However, it is very computationally inefficient to do this at run-time. Therefore, I propose to add a script 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).
• Please implement a 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.

https://github.com/tud-cor-sr/ros2-elastica/blob/testing_versions/src/ros2_elastica_pack/ros2_elastica_pack/examples/ContinuumFlagellaCase/continuum_flagella_ros2.py#L45

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:

1. Apply a torque tau to the system.
2. Wait for the system to reach steady-state
3. Compute the K with K=tau/q
4. We can do that for a few points, and identify our optimal fit for K with least-squares

Please 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:

https://github.com/tud-cor-sr/ros2-elastica/blob/main/elastica_sim_control/elastica_sim_control/elastica_publisher_subscriber.py#L123

https://github.com/tud-cor-sr/ros2-elastica/blob/436afc30f7909b9397da65641e277c39e012fb61/elastica_msgs/msg/RodState.msg#L19

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:

• The title of the repository should be formatted with a heading one (e.g. # Title)
• The sections of the README should be formatted with a heading two (e.g. ## Section Heading)
• Everything else and subsections should be formatted with a heading three (e.g. ### Subsection)
• Also your code-snippets someshave an empty line at the end. That looks a bit weird

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

https://github.com/tud-cor-sr/ros2-elastica/blob/436afc30f7909b9397da65641e277c39e012fb61/elastica_kinematics/elastica_kinematics/elastica_rod_states_to_PCC.py#L31

Rename no_of_segments to num_segments and add variable (currently set to 6) as a ROS2 variable

https://github.com/tud-cor-sr/ros2-elastica/blob/436afc30f7909b9397da65641e277c39e012fb61/elastica_kinematics/elastica_kinematics/elastica_rod_states_to_PCC.py#L30