Project: jiminy

Views: ²³⁹
License: MIT
Image: ubuntu2204

Kernel: Python 3 (system-wide)

In [5]:

# Setup the environment for google colab
try:
    from google.colab import output
    output.enable_custom_widget_manager()
    !pip install ipympl wurlitzer jiminy_py[meshcat] > /dev/null 2>&1
    !wget https://raw.githubusercontent.com/duburcqa/jiminy/demo/flexible_arm.urdf > /dev/null 2>&1
except ImportError:
    pass

In [6]:

pip install jiminy_py

Defaulting to user installation because normal site-packages is not writeable
Requirement already satisfied: jiminy_py in /usr/local/lib/python3.10/dist-packages (1.7.13)
Requirement already satisfied: typing-extensions>=3.10.0 in /usr/local/lib/python3.10/dist-packages (from jiminy_py) (4.5.0)
Requirement already satisfied: tqdm in /usr/local/lib/python3.10/dist-packages (from jiminy_py) (4.66.1)
WARNING: Retrying (Retry(total=4, connect=None, read=None, redirect=None, status=None)) after connection broken by 'ConnectTimeoutError(<pip._vendor.urllib3.connection.HTTPSConnection object at 0x7f15be7a7e50>, 'Connection to pypi.org timed out. (connect timeout=15)')': /simple/numpy/
WARNING: Retrying (Retry(total=3, connect=None, read=None, redirect=None, status=None)) after connection broken by 'ConnectTimeoutError(<pip._vendor.urllib3.connection.HTTPSConnection object at 0x7f15be7a6110>, 'Connection to pypi.org timed out. (connect timeout=15)')': /simple/numpy/
WARNING: Retrying (Retry(total=2, connect=None, read=None, redirect=None, status=None)) after connection broken by 'ConnectTimeoutError(<pip._vendor.urllib3.connection.HTTPSConnection object at 0x7f15be5b8130>, 'Connection to pypi.org timed out. (connect timeout=15)')': /simple/numpy/
WARNING: Retrying (Retry(total=1, connect=None, read=None, redirect=None, status=None)) after connection broken by 'ConnectTimeoutError(<pip._vendor.urllib3.connection.HTTPSConnection object at 0x7f15be5b9630>, 'Connection to pypi.org timed out. (connect timeout=15)')': /simple/numpy/
WARNING: Retrying (Retry(total=0, connect=None, read=None, redirect=None, status=None)) after connection broken by 'ConnectTimeoutError(<pip._vendor.urllib3.connection.HTTPSConnection object at 0x7f15be5b9ae0>, 'Connection to pypi.org timed out. (connect timeout=15)')': /simple/numpy/

In [0]:

import jiminy_py
jiminy_py.__version

In [2]:

# This module forward C++ errors to Python to help debugging
%load_ext wurlitzer

In [3]:

# Enable matplotlib interactive notebook integration
%matplotlib widget

---------------------------------------------------------------------------
ModuleNotFoundError                       Traceback (most recent call last)
/tmp/ipykernel_887/1964093056.py in <cell line: 2>()
      1 # Enable matplotlib interactive notebook integration
----> 2 get_ipython().run_line_magic('matplotlib', 'widget')

/usr/local/lib/python3.10/dist-packages/IPython/core/interactiveshell.py in run_line_magic(self, magic_name, line, _stack_depth)
   2416                 kwargs['local_ns'] = self.get_local_scope(stack_depth)
   2417             with self.builtin_trap:
-> 2418                 result = fn(*args, **kwargs)
   2419             return result
   2420 
/usr/local/lib/python3.10/dist-packages/decorator.py in fun(*args, **kw)
    230             if not kwsyntax:
    231                 args, kw = fix(args, kw, sig)
--> 232             return caller(func, *(extras + args), **kw)
    233     fun.__name__ = func.__name__
    234     fun.__doc__ = func.__doc__
/usr/local/lib/python3.10/dist-packages/IPython/core/magic.py in <lambda>(f, *a, **k)
    185     # but it's overkill for just that one bit of state.
    186     def magic_deco(arg):
--> 187         call = lambda f, *a, **k: f(*a, **k)
    188 
    189         if callable(arg):
/usr/local/lib/python3.10/dist-packages/IPython/core/magics/pylab.py in matplotlib(self, line)
     97             print("Available matplotlib backends: %s" % backends_list)
     98         else:
---> 99             gui, backend = self.shell.enable_matplotlib(args.gui.lower() if isinstance(args.gui, str) else args.gui)
    100             self._show_matplotlib_backend(args.gui, backend)
    101 
/usr/local/lib/python3.10/dist-packages/IPython/core/interactiveshell.py in enable_matplotlib(self, gui)
   3645                 gui, backend = pt.find_gui_and_backend(self.pylab_gui_select)
   3646 
-> 3647         pt.activate_matplotlib(backend)
   3648         configure_inline_support(self, backend)
   3649 
/usr/local/lib/python3.10/dist-packages/IPython/core/pylabtools.py in activate_matplotlib(backend)
    357     from matplotlib import pyplot as plt
    358 
--> 359     plt.switch_backend(backend)
    360 
    361     plt.show._needmain = False
/usr/local/lib/python3.10/dist-packages/matplotlib/pyplot.py in switch_backend(newbackend)
    269     old_backend = dict.__getitem__(rcParams, 'backend')
    270 
--> 271     backend_mod = importlib.import_module(
    272         cbook._backend_module_name(newbackend))
    273 
/usr/lib/python3.10/importlib/__init__.py in import_module(name, package)
    124                 break
    125             level += 1
--> 126     return _bootstrap._gcd_import(name[level:], package, level)
    127 
    128 
/usr/lib/python3.10/importlib/_bootstrap.py in _gcd_import(name, package, level)
/usr/lib/python3.10/importlib/_bootstrap.py in _find_and_load(name, import_)
/usr/lib/python3.10/importlib/_bootstrap.py in _find_and_load_unlocked(name, import_)
/usr/lib/python3.10/importlib/_bootstrap.py in _call_with_frames_removed(f, *args, **kwds)
/usr/lib/python3.10/importlib/_bootstrap.py in _gcd_import(name, package, level)
/usr/lib/python3.10/importlib/_bootstrap.py in _find_and_load(name, import_)
/usr/lib/python3.10/importlib/_bootstrap.py in _find_and_load_unlocked(name, import_)
ModuleNotFoundError: No module named 'ipympl'

In [0]:

import numpy as np
import matplotlib.pyplot as plt

import jiminy_py.core as jiminy  # The main module of jiminy - this is what gives access to the Robot
from jiminy_py.simulator import Simulator
from jiminy_py.log import extract_variables_from_log

In [0]:

# Reduce the waiting time before refreshing the viewer after closing for convenience
import jiminy_py.viewer.meshcat.server
jiminy_py.viewer.meshcat.server.WAIT_COM_TIMEOUT = 2.0

In [0]:

Flexible arm

Instantiate a robot

In [6]:

# First mount the drive
urdf_path = 'flexible_arm.urdf'
robot = jiminy.Robot()
robot.initialize(urdf_path, has_freeflyer=False)

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

# Add sensor
encoder = jiminy.EncoderSensor('active_joint')
robot.attach_sensor(encoder)
encoder.initialize('base_to_link1')

jiminy_py.core.hresult_t.SUCCESS

In [7]:

# We set inertia along non-moving axis to 1.0 for numerical stability
k_j, d_j = 100.0, 5.0
model_options = robot.get_model_options()
model_options['dynamics']['flexibilityConfig'] = [{
    'frameName': f"link{i}_to_link{i+1}",
    'stiffness': k_j * np.ones(3),
    'damping': d_j * np.ones(3),
    'inertia': np.array([1.0, 1.0, 0.0])
} for i in range(1,5)]
model_options['joints']['enablePositionLimit'] = False
model_options['joints']['enableVelocityLimit'] = False
robot.set_model_options(model_options)

jiminy_py.core.hresult_t.SUCCESS

Instantiate a controller

In [8]:

# Define the command: for now, the motor is off and doesn't modify the output torque.
Kp, Kd = 150.0, 5.0
q_ref = np.pi/4
def compute_command(t, q, v, sensors_data, command):
    q_a, v_a = sensors_data['EncoderSensor']
    command[:] = Kp * (q_ref - q_a) - Kd * v_a

In [9]:

# Instantiate and initialize the controller
controller = jiminy.ControllerFunctor(compute_command)
controller.initialize(robot)

jiminy_py.core.hresult_t.SUCCESS

Instantiate a simulator

In [10]:

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

In [11]:

# Configure the integrator
engine_options = simulator.engine.get_options()
engine_options['stepper']['controllerUpdatePeriod'] = 0.001
engine_options['stepper']['odeSolver'] = 'runge_kutta_4'
engine_options['stepper']['dtMax'] = 0.000125

# Make log files standalone for replay/post-processing on any machine
engine_options["telemetry"]["isPersistent"] = True
simulator.engine.set_options(engine_options)

jiminy_py.core.hresult_t.SUCCESS

In [12]:

# print out all the available engine options
engine_options

{'contacts': {'transitionVelocity': 0.01,
  'transitionEps': 0.001,
  'friction': 1.0,
  'stabilizationFreq': 20.0,
  'torsion': 0.0,
  'damping': 2000.0,
  'stiffness': 1000000.0,
  'model': 'constraint'},
 'constraints': {'successiveSolveFailedMax': 100,
  'regularization': 0.001,
  'solver': 'PGS'},
 'joints': {'boundDamping': 10000.0, 'boundStiffness': 10000000.0},
 'world': {'groundProfile': <jiminy_py.core.HeightmapFunctor at 0x7fc6baf36dc0>,
  'gravity': array([ 0.  ,  0.  , -9.81,  0.  ,  0.  ,  0.  ])},
 'stepper': {'logInternalStepperSteps': False,
  'controllerUpdatePeriod': 0.001,
  'iterMax': 0,
  'sensorsUpdatePeriod': 0.0,
  'successiveIterFailedMax': 1000,
  'timeout': 0.0,
  'dtRestoreThresholdRel': 0.2,
  'tolAbs': 1e-05,
  'dtMax': 0.000125,
  'tolRel': 0.0001,
  'odeSolver': 'runge_kutta_4',
  'randomSeed': 0,
  'verbose': False},
 'telemetry': {'enableEnergy': True,
  'enableMotorEffort': True,
  'enableCommand': True,
  'enableForceExternal': False,
  'enableAcceleration': True,
  'enableVelocity': True,
  'enableConfiguration': True,
  'isPersistent': True}}

Simulate the system

In [13]:

# Set initial condition and simulation length
q_rigid_init, v_rigid_init = np.array([0.]), np.array([0.])
duration = 2.0

# Launch the simulation
simulator.simulate(duration, q_rigid_init, v_rigid_init, is_state_theoretical=True)

In [14]:

# Replay the result
# Use backend 'meshcat' for interactive viewer (may be laggy), 'panda3d-sync' for video replay.
camera_pose = ([0.0, -1.8, 2.0e-5], [np.pi/2, 0.0, 0.0])
simulator.replay(camera_pose=camera_pose, speed_ratio=0.5, backend="panda3d-sync")

Rendering frames: 100%|██████████| 120/120 [00:00<00:00, 124.67it/s]
WARNING: Some output was deleted.

In [15]:

# Get dictionary of logged scalar variables
log_vars = simulator.log_data["variables"]

# Display the true position, velocity and effort of the actuated joint.
# The effort includes the internal dynamics if any (friction, plus custom if any).
_, (ax1, ax2, ax3) = plt.subplots(1, 3, figsize=(11, 3))
ax1.plot(log_vars['Global.Time'], log_vars[f'HighLevelController.currentPosition{motor_joint_name}'])
ax1.grid()
ax1.set_title('Position (rad)')
ax2.plot(log_vars['Global.Time'], log_vars[f'HighLevelController.currentVelocity{motor_joint_name}'])
ax2.grid()
ax2.set_title('Velocity (rad.s-1)')
ax3.plot(log_vars['Global.Time'], log_vars[f'HighLevelController.currentEffort{motor_joint_name}'])
ax3.grid()
ax3.set_title('Torque (N.m)')
plt.show();

Figure

In [16]:

# Extract the deformation angles along rotation axis
q_flex = []
for fj_name in robot.flexible_joints_names:
    quat = extract_variables_from_log(log_vars, [
        f"currentPosition{fj_name}Quat{e}" for e in ('X', 'Y', 'Z', 'W')
        ], 'HighLevelController')
    q_flex.append(2 * np.arctan2(quat[2], quat[3]))
q_flex = np.stack(q_flex, axis=0)

In [17]:

import pinocchio as pin
pin.rpy.matrixToRpy(pin.Quaternion(np.stack(quat, axis=1)[-1]).matrix())

array([ 0.       ,  0.       , -0.0031466])

In [18]:

# Display the deformation angles
plt.figure(figsize=(5, 3))
for k, q_i in enumerate(q_flex):
    plt.plot(log_vars['Global.Time'], q_i, label=f'q{k}')
plt.grid()
plt.legend()
plt.title('Flexible joints angles (rad)')
plt.show();

Figure

In [19]:

figure = simulator.plot(enable_flexiblity_data=True)

WARNING:root:Matplotlib's 'widget' and 'inline' backends are not properly supported. Please add '%matplotlib notebook' at the top and restart the kernel.

jiminy

In [0]: