Modelling flexible link robots using virtual link/joint method

[shamilmamedov]

Hi!
I want to model a flexible link robot by truncating the link into several virtual links connected by means of passive virtual joints with certain stiffness and damping (see the figure below, it explains the concept for single link robot). I have a URDF file that describes approximated model, but it doesn't contain joint stiffness and damping. I want to implement a simulator for training and RL agent. Is it possible to it using jiminy? Do you have any examples of flexible robots? What is the best place to start implementing flexible link robot simulator?

Thank you in advance!

d

[duburcqa]

Hi ! Yes it is perfectly doable, in fact it was partly designed for this specific use case. I don't have much time to help you at the moment but I will try to give you some insight on how to do it tomorrow.

[shamilmamedov]

Awesome! I don't need to reinvent the wheel then. Looking forward for insights from you.

[shamilmamedov]

@duburcqa, please let me know about insights that you mentioned as soon as you can

[duburcqa]

First, there is an example of simple RL environment based on Jiminy here. Next, a basic tutorial is available here. Finally, a sketch of document can be found here.

Regarding your specific setup, you can add deformation points either at the location of an actual joint or a fixed frame. In you case, it seems to be fixed frame. You need to adapt your kinematic tree to break up the single link in several smaller fixed rigidly attached to each other using "fixed" joints. Each of the sub-links must have their own visual and collision geometries. Then, once you URDF model is adapted you can start simulating flexibility very easily. To this end, two options must be configured:

model_options = robot.get_model_options()
model_options["dynamics"]["enableFlexibleModel"] = True
model_options["dynamics"]["flexibilityConfig"] = [{
    'frameName': "PendulumJoint",
    'stiffness': k * np.ones(3),
    'damping': nu * np.ones(3),
    'inertia': np.zeros(3)
}]
robot.set_model_options(model_options)

where model_options["dynamics"]["flexibilityConfig"] is a list of sub-dict that fully characterize a given deformation point. Here is a figure that describe the dynamical model. I can give you more details if you like.

Here is a very basic example to create a flexible model:

import os
import numpy as np
from pkg_resources import resource_filename

import jiminy_py.core as jiminy

# Get URDF path
data_dir = resource_filename("gym_jiminy.envs", "data/toys_models/cartpole")
urdf_path = os.path.join(data_dir, "cartpole.urdf")

# Instantiate robot
robot = jiminy.Robot()
robot.initialize(urdf_path, has_freeflyer=False, mesh_package_dirs=[data_dir])

# Add motors and sensors
motor_joint_name = "slider_to_cart"
encoder_sensors_descr = {
    "slider": "slider_to_cart",
    "pole": "cart_to_pole"
}
motor = jiminy.SimpleMotor(motor_joint_name)
robot.attach_motor(motor)
motor.initialize(motor_joint_name)
for sensor_name, joint_name in encoder_sensors_descr.items():
    encoder = jiminy.EncoderSensor(sensor_name)
    robot.attach_sensor(encoder)
    encoder.initialize(joint_name)

# Add deformation points
k, nu, Ia = 20.0, 0.1, 0.0
model_options = robot.get_model_options()
model_options["dynamics"]["enableFlexibleModel"] = True
model_options["dynamics"]["flexibilityConfig"] = [{
    'frameName': "PendulumJoint",
    'stiffness': k * np.ones(3),
    'damping': nu * np.ones(3),
    'inertia': Ia * np.ones(3)
}]
robot.set_model_options(model_options)

I recommend you to have a look to the set of all available options for both the engine and the model using get_options. I would give an overview of a subset of the features offered by Jiminy.

[shamilmamedov]

@duburcqa, thank you very much for detailed insights on implementing flexible robot simulator. Are you familiar with any other simulators/software for easy simulation flexible robots, so I can compare results of Jiminy with something external?

[duburcqa]

Are you familiar with any other simulators/software for easy simulation flexible robots, so I can compare results of Jiminy with something external?

Hum, I don't think there is any, probably because of the lack of consensus regarding the model. The one I'm using builds on the work of Matthieu Vigne and adds armature inertia to make sure the velocity is continuous. It enables to take larger integration time steps without diverging. The exact model requires Finite Element Analysis (FEA), which is way to costly for RL applications.

[shamilmamedov]

I see, thank you! I will close the issue for now.

[shamilmamedov]

Closed too early 😅

I installed Jiminy using pip install and while running tutorial with simple pendulum I get a segmentation fault error. What could be the reason? I have ROS (that uses EigenPy) and Pinocchio installed on my PC. Could there be conflicts between Pinocchio and Jiminy? How can I solve it?

On the positive side, I can successfully run an example on Google Colab. I created a URDF file with single active revolute joint and 3 fixed joints to simulate flexibilities. Then I defined enableFlexibleModel for one of them and tried to simulate the system. To this end I have a couple of question:

1. Is every flexible joint modeled as 3DOF spring? Can't I define only 1DOF spring?
2. What are states of a spring? Is it quaternion + ? Can choose spring positions ($q_s$) and velocities ($\dot q$) as states?

[duburcqa]

What could be the reason? I have ROS (that uses EigenPy) and Pinocchio installed on my PC. Could there be conflicts between Pinocchio and Jiminy?

Yes definitely, you need to use the same version that the one used to compile jiminy, or to compile jiminy yourself (which is quite straightforward). The right versions for the master branch are robotpkg-py3*-eigenpy=2.6.4 robotpkg-py3*-hpp-fcl=1.7.4r2 robotpkg-py3*-pinocchio=2.6.1, and for the dev branch robotpkg-py3*-eigenpy=2.7.6 robotpkg-py3*-hpp-fcl=1.8.1 robotpkg-py3*-pinocchio=2.6.7.

Is every flexible joint modeled as 3DOF spring?

Yes, but it should be the same since there is no forces applied in the two other directions (orthogonal to gravity according to your figure). Plus physically everything is 3D.

What are states of a spring? Is it quaternion + ? Can choose spring positions () and velocities (
) as states?

It follows pinocchio paradigm, so a quaternion (4 components) and a angular velocity (3 components).

Nota Bene: I would be happy if you could share your notebook, it could be a great addition to the few existing examples !

[shamilmamedov]

@duburcqa, thank you for your answer. I will gladly share my notebook once it works.

Sorry for bothering but I have new questions, I hope you can help me out.

Based on your previous comments I implemented a toy example, but can't simulate it, I get RuntimeError: The simulation failed without any hint on the source of failure. Here is an URDF file I used

<?xml version="1.0"?>
<robot name="flexible_arm">

<!-- world -->  
<link name="world"/>

<!-- flexible arm modeled using virtual link method -->  
<link name="link1">
  <visual>
    <origin rpy="0 0 0" xyz ="0.05 0 0"/>
    <geometry>
      <box size="0.1 0.025 0.01"/>
    </geometry>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz ="0.05 0 0"/>
    <geometry>
      <box size="0.1 0.025 0.01"/>
    </geometry>
  </collision>
  <inertial>
    <origin xyz="0.05 0 0" rpy="0 0 0"/>
    <mass value="0.8" />
    <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.002" />
  </inertial>
</link>

<joint name="base_to_link1" type="revolute">
  <parent link="world"/>
  <child link="link1"/>
  <axis xyz="0 0 1"/>
  <origin xyz="0 0 0" rpy="1.57 0 0"/>
  <limit effort="100.0" lower="-3.14159" upper="3.14159" velocity="3.14"/>
</joint>

<link name="link2">
  <visual>
    <origin rpy="0 0 0" xyz ="0.05 0 0"/>
    <geometry>
      <box size="0.1 0.025 0.01"/>
    </geometry>
  </visual>
  <collision>
    <origin rpy="0 0 0" xyz ="0.05 0 0"/>
    <geometry>
      <box size="0.1 0.025 0.01"/>
    </geometry>
  </collision>
  <inertial>
    <origin xyz="0.05 0 0" rpy="0 0 0"/>
    <mass value="0.8" />
    <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.002" />
  </inertial>
</link>

<joint name="link1_to_link2" type="fixed">
  <parent link="link1"/>
  <child link="link2"/>
  <axis xyz="0 0 1"/>
  <origin xyz="0.1 0 0" rpy="0 0 0"/>
  <limit effort="100.0" lower="-3.14159" upper="3.14159" velocity="3.14"/>
</joint>

</robot>

And a python code

# Instantiate a robot
fa_urdf_path = 'flexible_arm_jiminy.urdf'
fa = jiminy.Robot()
fa.initialize(fa_urdf_path, has_freeflyer=False)

# Add motor
motor_joint_name = 'base_to_link1'
motor = jiminy.SimpleMotor(motor_joint_name)
fa.attach_motor(motor)
motor.initialize(motor_joint_name)

# Specify joint flexibility parameters
model_options = fa.get_model_options()
model_options['dynamics']['enableFlexibleModel'] = True
model_options['dynamics']['flexibilityConfig'] = [{
    'frameName': 'link1_to_link2',
    'stiffness': 10.*np.ones(3),
    'damping': 1.*np.ones(3),
    'inertia': np.zeros(3)
}]
fa.set_model_options(model_options)

# Instantiate and initialize the controller
fa_controller = jiminy.ControllerFunctor()
fa_controller.initialize(fa)

# Create a simulator using this robot and controller
simulator = Simulator(fa, fa_controller)

# Set initial condition and simulation length
q0 = np.array([0., 0., 0., 1.])
v0 = np.array([0., 0., 0.])

print(f"q0 = {q0}")
print(f"v0 = {v0}")
simulation_duration = 10.0

# Launch the simulation
simulator.simulate(simulation_duration, q0, v0)

Am I missing something trivial? I tried to go through unittests to see how you simulate flexibility and didn't find anything useful for solving my problem.

[duburcqa]

If you use jupyter-notebook or similar, unfortunately std::err is forwarded to the terminal running it instead of the frontend. So you only see part of the message (the one written in native python) without the backtrace. To fix it, you need to install wurlitzer and load it into your notebook doing %load_ext wurlitzer. I will make it automatic in future release once it is supported on Windows.

You get none of the links have non-zero inertia along x axis, which is problematic if the flexibility is enabled because it could in theory rotate around them, even though it is never going to be the case by design. I will check if it is possible to fix it. If not, I will have a look to adding single-dof flexibility model as option.

[duburcqa]

After having a closer look to the issue, I think the best approach is just to set the armature inertia of the deformation points to a value larger than 0 on x axis, ie 1.

It is due to some compounding of errors causing non-zero acceleration around y-axis (0.1) that gets integrated slowly by slowly. I will check if it is possible to prevent it.

[shamilmamedov]

Thanks a lot @duburcqa now I see std::err messages. I was able to run simulation by changing inertial parameters of the links (set X and Y diagonal elements to 1) and passing only rigid joint states. If I pass both rigid and flexible joints states I get an error:

In /__w/jiminy/jiminy/core/src/robot/Model.cc:1738: In jiminy::Model::getFlexibleConfigurationFromRigid:
error: Size of qRigid inconsistent with theoretical model.

But I saw in your unit tests that you pass all the states. How come I can't do the same?

Another question: do you have any function that retrieves joint angles of the spherical flexible joints from quaternion?

[duburcqa]

I found the root cause of the compounding of errors, your approximation of pi/2 in the URDF file is not precise enough, you should rather use something like 1.57079632679489661923.

This part of the code is not properly tested for now, and your use case exhibits a bunch of bugs that I'm currently fixing (mainly affecting the visualization). I will write a unit test based on it if I have time to do it.

If I pass both rigid and flexible joints states I get an error: [...]

It is because your initial is just wrong because you have one actual revolute joint and a quaternion.
It should be: q0 = np.array([0., 0., 0., 0., 1.]) and v0 = np.array([0., 0., 0., 0.])

Another approach is to always provide the rigid state and let the simulator put the zero the deformation points:

q0 = np.array([0.])
v0 = np.array([0.])
simulator.simulate(simulation_duration, q0, v0, is_state_theoretical=True)

Another question: do you have any function that retrieves joint angles of the spherical flexible joints from quaternion?

Yes:

from gym_jiminy.toolbox.math import quat_to_yaw

q = simulator.system_state.q
flex_angles = []
for joint_idx in fa.flexible_joints_idx:
     idx_q = fa.pinocchio_model.joints[joint_idx].idx_q
     flex_angles.append(quat_to_yaw(q[idx_q:(idx_q+4)]))

[duburcqa]

PR #535 fixes a bunch of bugs. I have this complete example working:

<?xml version="1.0"?>
<robot name="flexible_arm">
  <!-- world -->
  <link name="world"/>

  <!-- flexible arm modeled using virtual link method -->
  <link name="link1">
    <visual>
      <origin rpy="0 0 0" xyz ="0.05 0 0"/>
      <geometry>
        <box size="0.1 0.025 0.01"/>
      </geometry>
      <material name="">
        <color rgba="1.0 0.0 0.0 1.0"/>
      </material>
    </visual>
    <inertial>
      <origin xyz="0.05 0 0" rpy="0 0 0"/>
      <mass value="0.8" />
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.002" />
    </inertial>
  </link>

  <joint name="base_to_link1" type="revolute">
    <parent link="world"/>
    <child link="link1"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0 0 0" rpy="1.57079632679489661923 0 0"/>
    <limit effort="100.0" lower="-3.14159" upper="3.14159" velocity="3.14"/>
  </joint>

  <link name="link2">
    <visual>
      <origin xyz ="0.05 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.025 0.01"/>
      </geometry>
      <material name="">
        <color rgba="0.0 1.0 0.0 1.0"/>
      </material>
    </visual>
    <inertial>
      <origin xyz="0.05 0 0" rpy="0 0 0"/>
      <mass value="0.8" />
      <inertia ixx="0" ixy="0" ixz="0" iyy="0.0" iyz="0" izz="0.002" />
    </inertial>
  </link>

  <joint name="link1_to_link2" type="fixed">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0.1 0 0" rpy="0 0 0"/>
  </joint>

  <link name="link3">
    <visual>
      <origin xyz ="0.05 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.025 0.01"/>
      </geometry>
      <material name="">
        <color rgba="0.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <inertial>
      <origin xyz="0.05 0 0" rpy="0 0 0"/>
      <mass value="0.8" />
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.002" />
    </inertial>
  </link>

  <joint name="link2_to_link3" type="fixed">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0.1 0 0" rpy="0 0 0"/>
  </joint>

  <link name="link4">
    <visual>
      <origin xyz ="0.05 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.1 0.025 0.01"/>
      </geometry>
      <material name="">
        <color rgba="1.0 0.0 1.0 1.0"/>
      </material>
    </visual>
    <inertial>
      <origin xyz="0.05 0 0" rpy="0 0 0"/>
      <mass value="0.8" />
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0.002" />
    </inertial>
  </link>

  <joint name="link3_to_link4" type="fixed">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="0.1 0 0" rpy="0 0 0"/>
  </joint>
</robot>

import numpy as np
import jiminy_py.core as jiminy
from jiminy_py.simulator import Simulator

# Instantiate a robot
fa_urdf_path = 'flexible_arm_jiminy.urdf'
fa = jiminy.Robot()
fa.initialize(fa_urdf_path, has_freeflyer=False)

# Add motor
motor_joint_name = 'base_to_link1'
motor = jiminy.SimpleMotor(motor_joint_name)
fa.attach_motor(motor)
motor.initialize(motor_joint_name)

# Specify joint flexibility parameters
model_options = fa.get_model_options()
model_options['dynamics']['enableFlexibleModel'] = True
model_options['dynamics']['flexibilityConfig'] = [{
    'frameName': f"link{i}_to_link{i+1}",
    'stiffness': .1 * np.ones(3),
    'damping': .1 * np.ones(3),
    'inertia': np.array([1.0, 1.0, 0.0])
} for i in range(1,4)]
fa.set_model_options(model_options)

# Instantiate and initialize the controller
fa_controller = jiminy.ControllerFunctor()
fa_controller.initialize(fa)

# Create a simulator using this robot and controller
simulator = Simulator(fa, fa_controller)

# Configure the integrator
# engine_options = simulator.engine.get_options()
# engine_options['stepper']['odeSolver'] = 'euler_explicit'
# engine_options['stepper']['dtMax'] = 0.001
# simulator.engine.set_options(engine_options)

# Set initial condition and simulation length
q0 = np.array([0.])
v0 = np.array([0.])

print(f"q0 = {q0}")
print(f"v0 = {v0}")
simulation_duration = 5.0

# Launch the simulation
simulator.simulate(simulation_duration, q0, v0, is_state_theoretical=True)

[duburcqa]

The PR #536 is adding unit tests to make sure it keeps working in the future. It should be merged on dev branch tomorrow morning.

I could release a new version on pypi if you need it.

[shamilmamedov]

Thank you very much for looking deeply into my issue. How can I use your latest changes in Jiminy? Do I need to pull devel branch and install it using setup.py or python3 -m pip install --prefer-binary jiminy-py[meshcat,plot] will suffice?

About initial state: even if I initialize it correctly with q0 = np.array([np.pi/4, 0., 0., 0., 1.]) and
v0 = np.array([0., 0., 0., 0.]) the simulator crashes.

A question about controllers: how can I define it? I try to do it this way

Kp = 150
Kd = 5
q_ref = np.pi/4
def compute_command(t, q, v, sensors_data, command):
    q_ref = np.pi/4
    command = Kp*(q_ref - q) - Kd*v

but I get broadcasting issues. Does simulator or controller know about active and passive flexible joints? Do they know that I only want to control active joints?

[duburcqa]

Do I need to pull devel branch and install it using setup.py or python3 -m pip install --prefer-binary jiminy-py[meshcat,plot] will suffice?

No, you need to follow the procedure to described in the documentation, or one of the CI scripts. Alternatively, you can download pre-compiled wheels for linux here.

the simulator crashes.

What is the error message ? You mean segfault ?

You cannot do command = Kp*(q_ref - q) - Kd*v because in python it means "overwrite the address this pointer is referring to by the one I'm providing you". It is never doing anything else in Python. In your case, you need to set the value not the pointer, so you need to do command[:] = Kp*(q_ref - q) - Kd*v.

I get broadcasting issues.

And yes you will get broadcasting issues because here q and v are associated with the full state, not the rigid one. You need to use the helper methods robot.get_rigid_configuration_from_flexible (and for velocity) before computing the command. Or even better, add a encoder sensor on the actuated joint, and use its data (position and velocity) to compute the command !

Here is a goody just for the fun (40 deformation points):
https://user-images.githubusercontent.com/17752950/173442063-305dd3d6-61e6-474e-b5ba-98e4432ba537.mp4

[shamilmamedov]

Hey, there. A new day new questions 😄

Wow, you simulation looks awesome; the object there looks like a rope.

I didn't get to which part of my question about installing your no is referred to, if I use Easy-install on Ubuntu 18+ will I get all the latest updates?

When specifying flexible link states I get

In /__w/jiminy/jiminy/core/src/robot/Model.cc:1738: In jiminy::Model::getFlexibleConfigurationFromRigid:
error: Size of qRigid inconsistent with theoretical model.

Regarding controller, I attached EncoderSensor to the link and used its states for feedback, thank you for the insight. Another question about controller: it is possible to pass time varying reference to it?

In Google Colab notebook I cannot get a proper visualization using this command

camera_xyzrpy = ([5.0, 0.0, 2.0e-5], [np.pi/2, 0.0, np.pi/2])
simulator.replay(camera_xyzrpy=camera_xyzrpy)

I get a couple of warnings ipykernel version and then the environment is opened as an output of the cell, the robot is in final configuration. When I want to stop the cell running kernel crushes. How can I solve it?

[duburcqa]

To get the last update, either you need to install to follow the instruction to build from source and checkout the dev branch, or you can just install the precompiled wheel available here. Just unpack the "wheelhouse" artefacts and run:

python3 -m pip install --prefer-binary --find-links=wheelhouse jiminy_py[meshcat,plot]

When specifying flexible link states I get

Do you have a complete example ? Because I'm quite sure there is no bug here.

Another question about controller: it is possible to pass time varying reference to it?

Yes, just create a proper controller rather than a simple callback, and use the time to fetch the right reference. You need to inherit from jiminy.BaseController and overload compute_command. You have an "example" here.

I get a couple of warnings ipykernel version and then the environment is opened as an output of the cell, the robot is in final configuration. When I want to stop the cell running kernel crushes. How can I solve it?

I think I have fixed those issues in the dev branch. Can you check if it works ?

[shamilmamedov]

I followed your instructions about getting the latest updates using "wheelhouse" but still notebook crashes while visualizing the robots motion. Here is the notebook I use, you can copy it and run to reproduce the issue.

[duburcqa]

First, you need to disable the browser tracking protection of firefox to make it work. But on my side, it still does not manage to connect to ipython for some reason. I will have a look to see if I can do something about it.

Besides, I would suggest to use simulator.write_log and jiminy_py.log.read_log to read/write logs. It is much more powerful than manual csv export. For instance you can make the log "persistent", so that you can replay it and do post-processing on any machine without access to the original URDF, meshes...etc I think it is critical for research to be able to backup experiments properly.

[duburcqa]

I think I found the issue with Google colab. I should be able to fix it soon. Reverse engineering how it works without docs or source code was painful.

Alternatively, you have the option to save it as a video using the options backend="panda3d-sync", record_video_path="video.mp4". Then display it in the notebook using:

from IPython.display import Video
Video("video.mp4")

[shamilmamedov]

Where do I need to set backend="panda3d-sync"? In simulator?

A question about simulator: before stumbling upon the Jiminy I wrote very small ad-hoc simulator for the flexible arm; there I also use pinocchio as a backend. In my code a robot is described using a URDF where passive joint that approximate flexibility of the link are revolute joints as well. In ODE to compute accelerations I call forward dynamics algorithm (aba) in pinocchio by passing joints positions, velocities and a torque vector $\tau = (\tau_a; \tau_p)$ where tau_a is a torque of an active joint provided by a controller and $\tau_p = - K q_p - D \dot q_p$ is spring-damper torque. tau_a is updated according to controller update period, while tau_p is continuous. Then in a loop I integrate the system starting from an initial condition using scipy's solve_ivp.

Once I had the Jiminy simulator running I wanted to compare its solution with mine simulators solution. All the conditions were the same: URDF is the same, controllers are the same . But, unfortunately, trajectories do not coincide, for example, the motion of flexible joints differ by an order of magnitude. I have been trying to find a bug in my code, but I haven't so far. How can I make sure that Jiminy provides correct solution? I can provide a link to my simulator if you need it.

What I found strange about Jiminy's solution is that in steady state when the active joint is rotated by pi/4 radians, all the passive joints (flexibility joints) have the same reading. But gravity acts on them differently, joints that are closer to the base should have bigger deflection because it bears more load. Correct me if I am wrong.

[duburcqa]

Where do I need to set backend="panda3d-sync"? In simulator?

in simulator.replay

How can I make sure that Jiminy provides correct solution? I can provide a link to my simulator if you need it.

First of all, you should configure both integrator to use the simple euler explicit schema. Jiminy is using the adaptive stepper Runge Kutta Dopri 5 by default. It is also possible to use RK45, which is fixed step but more complex that Euler Explicit. I would say the simpler the better. Yet It would be helpful if you could provide your simulator.

What I found strange about Jiminy's solution is that in steady state when the active joint is rotated by pi/4 radians, all the passive joints (flexibility joints) have the same reading.

Do you have a script to illustrate this ?

But gravity acts on them differently, joints that are closer to the base should have bigger deflection because it bears more load.

Yes indeed. Strange. There is no magic in the simulation here, which you described quite precisely in your message already. So I'm a bit puzzled.

[shamilmamedov]

The code for comparing trajectories in here in the script jiminy_vs_own.py. To compare simulated trajectories I first run Jiminy simulator in Colab (the one I sent yesterday) and save the solution in data folder; then I run script jiminy_vs_own.py. (I know it can be better than that 😀 )

Unfortunately, solve_ivp doesn't have euler explicit schema, it uses by default RK45. I set engine_options['stepper']['odeSolver'] = 'runge_kutta_4' but the integrator exploded.

Let me know if you have any why simulated trajectories are so different.

[duburcqa]

I have fixed the viewer in google colab. it is quite laggy but it works x) You need to update the ipykernel for it to work:

!pip install "ipykernel>=5,<6" > /dev/null

y the way, I suggest opening a view in one cell, and ask for replay in another one. This way, it will replay in the already existing cell, avoiding the open and close the view systematically, which is costly.

from jiminy_py.viewer import Viewer

Viewer.connect_backend()
Viewer.open_gui()

camera_xyzrpy = ([0.0, -1.5, 2.0e-5], [np.pi/2, 0.0, 0.0])
simulator.replay(camera_xyzrpy=camera_xyzrpy, speed_ratio=0.5)

[duburcqa]

but the integrator exploded.

Yes, you need to decrease the step size dtMax to use the same than your integrator.

[shamilmamedov]

I have control controllerUpdatePeriod = 0.001 in both simulators, I think it should limit integrators dtMax

[duburcqa]

The snippet to record and show video:

from base64 import b64encode
from IPython.display import HTML

camera_xyzrpy = ([0.0, -1.5, 2.0e-5], [np.pi/2, 0.0, 0.0])
simulator.replay(camera_xyzrpy=camera_xyzrpy, record_video_path="video.mp4", backend="panda3d-sync")

mp4 = open('video.mp4','rb').read()
HTML(f"""
<video width=400 controls>
  <source src="{ "data:video/mp4;base64," + b64encode(mp4).decode()}" type="video/mp4">
</video>
""")

I have control controllerUpdatePeriod = 0.001 in both simulators, I think it should limit integrators dtMax

Yes it does, but 1ms may still be too large without armature inertia apparently. It is a bit surprising but definitely not impossible. You should increase the flexibility inertia to smooth that out.

[shamilmamedov]

Thank you for solving issues with a visualization. I will try it out tomorrow.

Regarding simulation: I decreased dtMax to 0.00025, it converges but again all the positions of the flexible joints are the same.

[duburcqa]

Regarding simulation: I decreased dtMax to 0.00025, it converges but again all the positions of the flexible joints are the same.

Interesting. How can I check that myself ? From the video based on your "original" notebook it looks quite right.

[shamilmamedov]

By doing what I do -- described here.

There is a chance that I am doing something wrong: maybe converting quaternions to angles or something entirely else.

[duburcqa]

Hum, I would say yes there is a mistake with your extract of the result. Because here is the video:
https://user-images.githubusercontent.com/17752950/173930194-0b609a2a-26b3-425d-b576-2a56d2cc1e29.mp4

To me it looks exactly as it should.

Nota bene: You will find the latest wheels to download here once it is done compiling (it takes about 45min).

[shamilmamedov]

Here is a function I use for extracting flexible joint positions

def get_flexible_joints_positions_from_log(log_data, fa):
    ns = log_data['Global.Time'].size
    nfj = len(fa.flexible_joints_names)
    qf = np.zeros((ns, nfj))
    for k, (fj_name, fj_idx) in enumerate(zip(fa.flexible_joints_names, fa.flexible_joints_idx)):
        quat = np.column_stack((log_data[f'HighLevelController.currentPosition{fj_name}QuatX'],
                                log_data[f'HighLevelController.currentPosition{fj_name}QuatY'],
                                log_data[f'HighLevelController.currentPosition{fj_name}QuatZ'],
                                log_data[f'HighLevelController.currentPosition{fj_name}QuatW']))
        idx_q = fa.pinocchio_model.joints[fj_idx].idx_q
        for j in range(ns):
            qf[j,k] = quat_to_yaw(quat[j])
    return qf

[duburcqa]

Your extract is ok, but it this example, the arm is almost right, presumably because the flexibility are much stronger. I think you cannot compare your stiffness with the one of jiminy easily. here is the formula:

        // Compute the flexibilities (only support joint_t::SPHERICAL so far)
        Robot::dynamicsOptions_t const & mdlDynOptions = system.robot->mdlOptions_->dynamics;
        std::vector<jointIndex_t> const & flexibilityIdx = system.robot->getFlexibleJointsModelIdx();
        for (std::size_t i = 0; i < flexibilityIdx.size(); ++i)
        {
            jointIndex_t const & jointIdx = flexibilityIdx[i];
            uint32_t const & positionIdx = pncModel.joints[jointIdx].idx_q();
            uint32_t const & velocityIdx = pncModel.joints[jointIdx].idx_v();
            vector3_t const & stiffness = mdlDynOptions.flexibilityConfig[i].stiffness;
            vector3_t const & damping = mdlDynOptions.flexibilityConfig[i].damping;

            quaternion_t const quat(q.segment<4>(positionIdx));  // Only way to initialize with [x,y,z,w] order
            vector3_t const axis = pinocchio::quaternion::log3(quat);
            uInternal.segment<3>(velocityIdx).array() +=
                - stiffness.array() * axis.array()
                - damping.array() * v.segment<3>(velocityIdx).array();
        }

video2.mp4

You may want to disable the position and velocity bounds in jiminy, because I assume you did not implement them in your simulator.

[duburcqa]

I will release a new version of jiminy tonight. For now, you can get it here if you want. I have fixed the interactive viewer mode in google colab, but since it is laggy and quite fragile (it works better in mybinder) I have added a video mode that is used by default instead.

[shamilmamedov]

About flexibility, I still don't understand why visualization and reading from log_data do not match? In simulation I disabled position and velocity limits as well as effort limit.

Is it too difficult to add 1D spring to Jiminy?

I will try new version tomorrow.

[duburcqa]

About flexibility, I still don't understand why visualization and reading from log_data do not match?

You must have a bug somewhere because I'm sure both matches. You can reply log files with jiminy using jiminy_py.viewer.play_logs_files to make sure it is fine.

Is it too difficult to add 1D spring to Jiminy?

It is not about how difficult it is. It is just that it does not make sense physically since the deformation are never 1D in reality. And how do you even choose the axis ? Fixed frame do not even have an axis in standard URDF specification, only an origin. It makes sense for 1D motors and some authors are doing it in the literature but it is not generic. Moreover, it is no advantage over 3D spring in terms of computation speed or physical validity. Pinocchio (and Jiminy) is not made with planar robots in mind but for real robots in the first place.

[duburcqa]

I have released a new version of jiminy and made your problem a demo here.

[duburcqa]

By the way, you could implement yourself the model you like by replacing fixed joints by revolute, adding "motors" to them and defining an internal dynamics for them (through internal_dynamics of the controller not compute_command) instead of actual flexibility. I think it would more convenient for you, since the state would be the one you expect and the physics as well. The internal dynamics is continuously evaluated while the command is periodically updated. If you like it, the callback should be implemented as a C++ extension of jiminy though, because otherwise it is likely to be very slow due to going back and forth repeatedly from Python to C++.

[duburcqa]

I'm going to have a look to motor transmission flexibility (what you have in mind) today and do a comparison between the two models to see how different it is.

[shamilmamedov]

@duburcqa Thanks a lot for trying to solve my issues. I didn't have time to work on jiminy implementation of my setup, and probably won't have time in the weekend. I will get back to flexible link simulation ASAP

[duburcqa]

No problem, just I will not have time to work on improving jiminy until august after today.