From 1ae998e5aef7dacc13bad7aa457575a513d78864 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Mon, 26 Aug 2024 11:34:28 +0200
Subject: [PATCH] ruff automatic fixes
---
...id_talos_gripper_closed_kinematic_chain.py | 3 +-
exercizes/ex_1_ur5.py | 12 +-
exercizes/ex_3_biped_balance_with_gui.py | 14 +-
exercizes/ex_4_conf.py | 4 +-
exercizes/ex_4_long_conf.py | 4 +-
exercizes/ex_4_talos_conf.py | 4 +-
...x_0_joint_space_inverse_dynamics_arm.ipynb | 116 +++++----
.../notebooks/ex_1_com_sin_track_talos.ipynb | 232 +++++++++++-------
.../ex_3_biped_balance_with_gui.ipynb | 86 ++++---
exercizes/plot_utils.py | 4 +-
exercizes/romeo_conf.py | 4 +-
exercizes/test_cop_task.py | 14 +-
exercizes/ur5_conf.py | 4 +-
exercizes/ur5_reaching_conf.py | 4 +-
tests/python/generator.py | 5 +-
15 files changed, 303 insertions(+), 207 deletions(-)
diff --git a/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
index fa6b3be..f3f1fc5 100644
--- a/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
+++ b/demo/demo_tsid_talos_gripper_closed_kinematic_chain.py
@@ -1,5 +1,4 @@
-"""
-Simple demo of usage of ContactTwoFramePositions in TSID
+"""Simple demo of usage of ContactTwoFramePositions in TSID
Make the Talos gripper model works with closed kinematic chains
(c) MIPT
"""
diff --git a/exercizes/ex_1_ur5.py b/exercizes/ex_1_ur5.py
index ea6a549..b737c99 100644
--- a/exercizes/ex_1_ur5.py
+++ b/exercizes/ex_1_ur5.py
@@ -10,9 +10,9 @@ from numpy import nan
from numpy.linalg import norm as norm
from tsid_manipulator import TsidManipulator
-print(("".center(conf.LINE_WIDTH, "#")))
-print((" TSID - Manipulator End-Effector Sin Tracking ".center(conf.LINE_WIDTH, "#")))
-print(("".center(conf.LINE_WIDTH, "#")))
+print("".center(conf.LINE_WIDTH, "#"))
+print(" TSID - Manipulator End-Effector Sin Tracking ".center(conf.LINE_WIDTH, "#"))
+print("".center(conf.LINE_WIDTH, "#"))
print("")
PLOT_EE_POS = 1
@@ -89,12 +89,12 @@ for i in range(0, N):
ee_acc_des[:, i] = tsid.eeTask.getDesiredAcceleration[:3]
if i % conf.PRINT_N == 0:
- print(("Time %.3f" % (t)))
+ print("Time %.3f" % (t))
print(
- (
+
"\ttracking err %s: %.3f"
% (tsid.eeTask.name.ljust(20, "."), norm(tsid.eeTask.position_error, 2))
- )
+
)
q[:, i + 1], v[:, i + 1] = tsid.integrate_dv(q[:, i], v[:, i], dv, conf.dt)
diff --git a/exercizes/ex_3_biped_balance_with_gui.py b/exercizes/ex_3_biped_balance_with_gui.py
index 7412363..6787bba 100644
--- a/exercizes/ex_3_biped_balance_with_gui.py
+++ b/exercizes/ex_3_biped_balance_with_gui.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Wed Apr 17 22:31:22 2019
+"""Created on Wed Apr 17 22:31:22 2019
@author: student
"""
@@ -116,8 +114,14 @@ def push_robot():
def create_gui():
- """thread worker function"""
- global scale_com, scale_RF, scale_LF, button_contact_RF, button_contact_LF, com_vel_entry
+ """Thread worker function"""
+ global \
+ scale_com, \
+ scale_RF, \
+ scale_LF, \
+ button_contact_RF, \
+ button_contact_LF, \
+ com_vel_entry
master = Tk(className="TSID GUI")
scale_com = Scale3d(
master,
diff --git a/exercizes/ex_4_conf.py b/exercizes/ex_4_conf.py
index 74f7a86..cf0ab21 100644
--- a/exercizes/ex_4_conf.py
+++ b/exercizes/ex_4_conf.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
diff --git a/exercizes/ex_4_long_conf.py b/exercizes/ex_4_long_conf.py
index c6cacd4..bec24a6 100644
--- a/exercizes/ex_4_long_conf.py
+++ b/exercizes/ex_4_long_conf.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
diff --git a/exercizes/ex_4_talos_conf.py b/exercizes/ex_4_talos_conf.py
index 0f76b11..8affe01 100644
--- a/exercizes/ex_4_talos_conf.py
+++ b/exercizes/ex_4_talos_conf.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
diff --git a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
index 04eee69..89bf0d8 100644
--- a/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
+++ b/exercizes/notebooks/ex_0_joint_space_inverse_dynamics_arm.ipynb
@@ -1,5 +1,5 @@
{
- "cells": [
+ "cells": [
{
"cell_type": "markdown",
"metadata": {},
@@ -16,7 +16,8 @@
"outputs": [],
"source": [
"import sys\n",
- "sys.path.append('..')\n",
+ "\n",
+ "sys.path.append(\"..\")\n",
"\n",
"import numpy as np\n",
"from numpy.linalg import norm\n",
@@ -25,8 +26,8 @@
"import time\n",
"import pinocchio as pin\n",
"import tsid\n",
- "#import gepetto.corbaserver\n",
- "import subprocess\n",
+ "\n",
+ "# import gepetto.corbaserver\n",
"import os\n",
"\n",
"import talos_arm_conf as conf"
@@ -124,7 +125,9 @@
"jointBoundsTask = tsid.TaskJointBounds(\"task-joint-bounds\", robot, conf.dt)\n",
"jointBoundsTask.setVelocityBounds(v_min, v_max)\n",
"priorityLevel = 0\n",
- "formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, priorityLevel, transitionTime)"
+ "formulation.addMotionTask(\n",
+ " jointBoundsTask, conf.w_joint_bounds, priorityLevel, transitionTime\n",
+ ")"
]
},
{
@@ -157,10 +160,14 @@
"metadata": {},
"outputs": [],
"source": [
- "robot_display = pin.RobotWrapper.BuildFromURDF(conf.urdf, [os.path.join(conf.path, '../..')])\n",
- "#Viewer = pin.visualize.GepettoVisualizer\n",
+ "robot_display = pin.RobotWrapper.BuildFromURDF(\n",
+ " conf.urdf, [os.path.join(conf.path, \"../..\")]\n",
+ ")\n",
+ "# Viewer = pin.visualize.GepettoVisualizer\n",
"Viewer = pin.visualize.MeshcatVisualizer\n",
- "viz = Viewer(robot_display.model, robot_display.collision_model, robot_display.visual_model)\n",
+ "viz = Viewer(\n",
+ " robot_display.model, robot_display.collision_model, robot_display.visual_model\n",
+ ")\n",
"viz.initViewer(loadModel=True)\n",
"viz.display(q0)"
]
@@ -171,7 +178,7 @@
"metadata": {},
"outputs": [],
"source": [
- "hasattr(viz.viewer, 'jupyter_cell') and viz.viewer.jupyter_cell()"
+ "hasattr(viz.viewer, \"jupyter_cell\") and viz.viewer.jupyter_cell()"
]
},
{
@@ -188,12 +195,12 @@
"outputs": [],
"source": [
"N = conf.N_SIMULATION\n",
- "tau = np.full((robot.na, N), np.nan)\n",
- "q = np.full((robot.nq, N + 1), np.nan)\n",
- "v = np.full((robot.nv, N + 1), np.nan)\n",
- "dv = np.full((robot.nv, N + 1), np.nan)\n",
- "q_ref = np.full((robot.nq, N), np.nan)\n",
- "v_ref = np.full((robot.nv, N), np.nan)\n",
+ "tau = np.full((robot.na, N), np.nan)\n",
+ "q = np.full((robot.nq, N + 1), np.nan)\n",
+ "v = np.full((robot.nv, N + 1), np.nan)\n",
+ "dv = np.full((robot.nv, N + 1), np.nan)\n",
+ "q_ref = np.full((robot.nq, N), np.nan)\n",
+ "v_ref = np.full((robot.nv, N), np.nan)\n",
"dv_ref = np.full((robot.nv, N), np.nan)\n",
"dv_des = np.full((robot.nv, N), np.nan)\n",
"samplePosture = trajPosture.computeNext()"
@@ -212,10 +219,12 @@
"metadata": {},
"outputs": [],
"source": [
- "amp = np.array([0.2, 0.3, 0.4, 0.0, 0.0, 0.0, 0.0]) # amplitude\n",
- "phi = np.array([0.0, 0.5 * np.pi, 0.0, 0.0, 0.0, 0.0, 0.0]) # phase\n",
- "two_pi_f = 2 * np.pi * np.array([1.0, 0.5, 0.3, 0.0, 0.0, 0.0, 0.0]) # frequency (time 2 PI)\n",
- "two_pi_f_amp = np.multiply(two_pi_f, amp)\n",
+ "amp = np.array([0.2, 0.3, 0.4, 0.0, 0.0, 0.0, 0.0]) # amplitude\n",
+ "phi = np.array([0.0, 0.5 * np.pi, 0.0, 0.0, 0.0, 0.0, 0.0]) # phase\n",
+ "two_pi_f = (\n",
+ " 2 * np.pi * np.array([1.0, 0.5, 0.3, 0.0, 0.0, 0.0, 0.0])\n",
+ ") # frequency (time 2 PI)\n",
+ "two_pi_f_amp = np.multiply(two_pi_f, amp)\n",
"two_pi_f_squared_amp = np.multiply(two_pi_f, two_pi_f_amp)"
]
},
@@ -242,38 +251,41 @@
"\n",
"for i in range(N):\n",
" time_start = time.time()\n",
- " \n",
+ "\n",
" # set reference trajectory\n",
- " q_ref[:,i] = q0 + amp * np.sin(two_pi_f * t + phi)\n",
- " v_ref[:,i] = two_pi_f_amp * np.cos(two_pi_f * t + phi)\n",
- " dv_ref[:,i] = -two_pi_f_squared_amp * np.sin(two_pi_f * t + phi)\n",
+ " q_ref[:, i] = q0 + amp * np.sin(two_pi_f * t + phi)\n",
+ " v_ref[:, i] = two_pi_f_amp * np.cos(two_pi_f * t + phi)\n",
+ " dv_ref[:, i] = -two_pi_f_squared_amp * np.sin(two_pi_f * t + phi)\n",
" samplePosture.pos(q_ref[:, i])\n",
" samplePosture.vel(v_ref[:, i])\n",
" samplePosture.acc(dv_ref[:, i])\n",
" postureTask.setReference(samplePosture)\n",
"\n",
- " HQPData = formulation.computeProblemData(t, q[:,i], v[:,i])\n",
+ " HQPData = formulation.computeProblemData(t, q[:, i], v[:, i])\n",
" sol = solver.solve(HQPData)\n",
" if sol.status != 0:\n",
" print(\"Time %.3f QP problem could not be solved! Error code:\" % t, sol.status)\n",
" break\n",
- " \n",
- " tau[:,i] = formulation.getActuatorForces(sol)\n",
- " dv[:,i] = formulation.getAccelerations(sol)\n",
- " dv_des[:,i] = postureTask.getDesiredAcceleration\n",
+ "\n",
+ " tau[:, i] = formulation.getActuatorForces(sol)\n",
+ " dv[:, i] = formulation.getAccelerations(sol)\n",
+ " dv_des[:, i] = postureTask.getDesiredAcceleration\n",
"\n",
" if i % conf.PRINT_N == 0:\n",
" print(\"Time %.3f\" % (t))\n",
- " print(\"\\ttracking err %s: %.3f\" % (postureTask.name.ljust(20, '.'), norm(postureTask.position_error, 2)))\n",
+ " print(\n",
+ " \"\\ttracking err %s: %.3f\"\n",
+ " % (postureTask.name.ljust(20, \".\"), norm(postureTask.position_error, 2))\n",
+ " )\n",
"\n",
" # numerical integration\n",
- " v_mean = v[:,i] + 0.5*dt*dv[:,i]\n",
- " v[:,i + 1] = v[:,i] + dt * dv[:,i]\n",
- " q[:,i + 1] = pin.integrate(model, q[:,i], dt * v_mean)\n",
+ " v_mean = v[:, i] + 0.5 * dt * dv[:, i]\n",
+ " v[:, i + 1] = v[:, i] + dt * dv[:, i]\n",
+ " q[:, i + 1] = pin.integrate(model, q[:, i], dt * v_mean)\n",
" t += conf.dt\n",
- " \n",
+ "\n",
" if i % conf.DISPLAY_N == 0:\n",
- " viz.display(q[:,i])\n",
+ " viz.display(q[:, i])\n",
"\n",
" time_spent = time.time() - time_start\n",
" if time_spent < conf.dt:\n",
@@ -291,10 +303,10 @@
"source": [
"(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
"for i in range(robot.nv):\n",
- " ax[i].plot(time, q[i,:-1], label='q %i' % i)\n",
- " ax[i].plot(time, q_ref[i,:], '--', label='q ref %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('q [rad]')\n",
+ " ax[i].plot(time, q[i, :-1], label=\"q %i\" % i)\n",
+ " ax[i].plot(time, q_ref[i, :], \"--\", label=\"q ref %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"q [rad]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"plt.show()"
@@ -308,12 +320,12 @@
"source": [
"(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
"for i in range(robot.nv):\n",
- " ax[i].plot(time, v[i,:-1], label='v %i ' % i)\n",
- " ax[i].plot(time, v_ref[i,:], '--', label='v ref %i' % i)\n",
- " ax[i].plot([time[0], time[-1]], 2 * [v_min[i]], ':')\n",
- " ax[i].plot([time[0], time[-1]], 2 * [v_max[i]], ':')\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('v [rad/s]')\n",
+ " ax[i].plot(time, v[i, :-1], label=\"v %i \" % i)\n",
+ " ax[i].plot(time, v_ref[i, :], \"--\", label=\"v ref %i\" % i)\n",
+ " ax[i].plot([time[0], time[-1]], 2 * [v_min[i]], \":\")\n",
+ " ax[i].plot([time[0], time[-1]], 2 * [v_max[i]], \":\")\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"v [rad/s]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"plt.show()"
@@ -327,11 +339,11 @@
"source": [
"(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
"for i in range(robot.nv):\n",
- " ax[i].plot(time, dv[i,:-1], label='dv '+str(i))\n",
- " ax[i].plot(time, dv_ref[i,:], '--', label='dv ref %i' % i)\n",
- " ax[i].plot(time, dv_des[i,:], ':', label='dv des %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('dv [rad/s^2]')\n",
+ " ax[i].plot(time, dv[i, :-1], label=\"dv \" + str(i))\n",
+ " ax[i].plot(time, dv_ref[i, :], \"--\", label=\"dv ref %i\" % i)\n",
+ " ax[i].plot(time, dv_des[i, :], \":\", label=\"dv des %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"dv [rad/s^2]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"plt.show()"
@@ -345,9 +357,9 @@
"source": [
"(f, ax) = plut.create_empty_figure(robot.nv, 1, figsize=(10, 20))\n",
"for i in range(robot.nv):\n",
- " ax[i].plot(time, tau[i,:], label='tau %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('tau [Nm]')\n",
+ " ax[i].plot(time, tau[i, :], label=\"tau %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"tau [Nm]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"plt.show()"
diff --git a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
index 1d707a6..1d13910 100644
--- a/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
+++ b/exercizes/notebooks/ex_1_com_sin_track_talos.ipynb
@@ -1,5 +1,5 @@
{
- "cells": [
+ "cells": [
{
"cell_type": "markdown",
"metadata": {},
@@ -241,13 +241,16 @@
"\n",
"# import the library TSID for the Whole-Body Controller\n",
"import tsid\n",
+ "\n",
"# import the pinocchio library for the mathematical methods (Lie algebra) and multi-body dynamics computations.\n",
"import pinocchio as pin\n",
+ "\n",
"# import example_example_robot_data to get the current path for the robot models\n",
"from example_robot_data.path import EXAMPLE_ROBOT_DATA_MODEL_DIR\n",
"\n",
"import sys\n",
- "sys.path.append('..')\n",
+ "\n",
+ "sys.path.append(\"..\")\n",
"\n",
"# import graphical tools\n",
"import plot_utils as plut"
@@ -261,40 +264,70 @@
"source": [
"# Definition of the tasks gains, weights and the foot geometry (for contact task)\n",
"\n",
- "lxp = 0.1 # foot length in positive x direction\n",
- "lxn = 0.11 # foot length in negative x direction\n",
- "lyp = 0.069 # foot length in positive y direction\n",
- "lyn = 0.069 # foot length in negative y direction\n",
- "lz = 0.107 # foot sole height with respect to ankle joint\n",
- "mu = 0.3 # friction coefficient\n",
- "fMin = 1.0 # minimum normal force\n",
+ "lxp = 0.1 # foot length in positive x direction\n",
+ "lxn = 0.11 # foot length in negative x direction\n",
+ "lyp = 0.069 # foot length in positive y direction\n",
+ "lyn = 0.069 # foot length in negative y direction\n",
+ "lz = 0.107 # foot sole height with respect to ankle joint\n",
+ "mu = 0.3 # friction coefficient\n",
+ "fMin = 1.0 # minimum normal force\n",
"fMax = 1000.0 # maximum normal force\n",
"\n",
- "rf_frame_name = \"leg_right_6_joint\" # right foot joint name\n",
- "lf_frame_name = \"leg_left_6_joint\" # left foot joint name\n",
- "contactNormal = np.array([0., 0., 1.]) # direction of the normal to the contact surface\n",
+ "rf_frame_name = \"leg_right_6_joint\" # right foot joint name\n",
+ "lf_frame_name = \"leg_left_6_joint\" # left foot joint name\n",
+ "contactNormal = np.array(\n",
+ " [0.0, 0.0, 1.0]\n",
+ ") # direction of the normal to the contact surface\n",
"\n",
- "w_com = 1.0 # weight of center of mass task\n",
- "w_posture = 0.1 # weight of joint posture task\n",
+ "w_com = 1.0 # weight of center of mass task\n",
+ "w_posture = 0.1 # weight of joint posture task\n",
"w_forceRef = 1e-3 # weight of force regularization task\n",
- "w_waist = 1.0 # weight of waist task\n",
+ "w_waist = 1.0 # weight of waist task\n",
"\n",
"kp_contact = 30.0 # proportional gain of contact constraint\n",
- "kp_com = 20.0 # proportional gain of center of mass task\n",
- "kp_waist = 500.0 # proportional gain of waist task\n",
- "\n",
- "kp_posture = np.array( # proportional gain of joint posture task\n",
- " [10., 5., 5., 1., 1., 10., # lleg, low gain on axis along y and knee\n",
- " 10., 5., 5., 1., 1., 10., # rleg\n",
- " 500., 500., # chest\n",
- " 50., 10., 10., 10., 10., 10., 10., 10., # larm\n",
- " 50., 10., 10., 10., 10., 10., 10., 10., # rarm\n",
- " 100., 100.] # head\n",
+ "kp_com = 20.0 # proportional gain of center of mass task\n",
+ "kp_waist = 500.0 # proportional gain of waist task\n",
+ "\n",
+ "kp_posture = np.array( # proportional gain of joint posture task\n",
+ " [\n",
+ " 10.0,\n",
+ " 5.0,\n",
+ " 5.0,\n",
+ " 1.0,\n",
+ " 1.0,\n",
+ " 10.0, # lleg, low gain on axis along y and knee\n",
+ " 10.0,\n",
+ " 5.0,\n",
+ " 5.0,\n",
+ " 1.0,\n",
+ " 1.0,\n",
+ " 10.0, # rleg\n",
+ " 500.0,\n",
+ " 500.0, # chest\n",
+ " 50.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0, # larm\n",
+ " 50.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0,\n",
+ " 10.0, # rarm\n",
+ " 100.0,\n",
+ " 100.0,\n",
+ " ] # head\n",
")\n",
"\n",
- "dt = 0.001 # controller time step\n",
- "PRINT_N = 500 # print every PRINT_N time steps\n",
- "DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps\n",
+ "dt = 0.001 # controller time step\n",
+ "PRINT_N = 500 # print every PRINT_N time steps\n",
+ "DISPLAY_N = 20 # update robot configuration in viwewer every DISPLAY_N time steps\n",
"N_SIMULATION = 10000 # number of time steps simulated"
]
},
@@ -306,10 +339,10 @@
"source": [
"# Set the path where the urdf file of the robot is registered\n",
"path = EXAMPLE_ROBOT_DATA_MODEL_DIR\n",
- "urdf = path + '/talos_data/robots/talos_reduced.urdf'\n",
+ "urdf = path + \"/talos_data/robots/talos_reduced.urdf\"\n",
"# Create the robot wrapper from the urdf, it will give the model of the robot and its data\n",
"robot = tsid.RobotWrapper(urdf, [path], pin.JointModelFreeFlyer(), False)\n",
- "srdf = path + '/talos_data/srdf/talos.srdf'"
+ "srdf = path + \"/talos_data/srdf/talos.srdf\""
]
},
{
@@ -319,10 +352,14 @@
"outputs": [],
"source": [
"# Creation of the robot wrapper for gepetto viewer (graphical interface)\n",
- "robot_display = pin.RobotWrapper.BuildFromURDF(urdf, [os.path.join(path, '../..')], pin.JointModelFreeFlyer())\n",
- "#Viewer = pin.visualize.GepettoVisualizer\n",
+ "robot_display = pin.RobotWrapper.BuildFromURDF(\n",
+ " urdf, [os.path.join(path, \"../..\")], pin.JointModelFreeFlyer()\n",
+ ")\n",
+ "# Viewer = pin.visualize.GepettoVisualizer\n",
"Viewer = pin.visualize.MeshcatVisualizer\n",
- "viz = Viewer(robot_display.model, robot_display.collision_model, robot_display.visual_model)\n",
+ "viz = Viewer(\n",
+ " robot_display.model, robot_display.collision_model, robot_display.visual_model\n",
+ ")\n",
"viz.initViewer(loadModel=True)"
]
},
@@ -336,7 +373,7 @@
"model = robot.model()\n",
"pin.loadReferenceConfigurations(model, srdf, False)\n",
"# Set the current configuration q to the robot configuration half_sitting\n",
- "q = model.referenceConfigurations['half_sitting']\n",
+ "q = model.referenceConfigurations[\"half_sitting\"]\n",
"# Set the current velocity to zero\n",
"v = np.zeros(robot.nv)"
]
@@ -436,21 +473,23 @@
"source": [
"# COM Task\n",
"comTask = tsid.TaskComEquality(\"task-com\", robot)\n",
- "comTask.setKp(kp_com * np.ones(3)) # Proportional gain defined before = 20\n",
- "comTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(3)) # Derivative gain = 2 * sqrt(20)\n",
+ "comTask.setKp(kp_com * np.ones(3)) # Proportional gain defined before = 20\n",
+ "comTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(3)) # Derivative gain = 2 * sqrt(20)\n",
"# Add the task to the HQP with weight = 1.0, priority level = 0 (as constraint) and a transition duration = 0.0\n",
"invdyn.addMotionTask(comTask, w_com, 0, 0.0)\n",
"\n",
"# WAIST Task\n",
- "waistTask = tsid.TaskSE3Equality(\"task-waist\", robot, 'root_joint') # waist -> root_joint\n",
- "waistTask.setKp(kp_waist * np.ones(6)) # Proportional gain defined before = 500\n",
- "waistTask.setKd(2.0 * np.sqrt(kp_waist) * np.ones(6)) # Derivative gain = 2 * sqrt(500)\n",
+ "waistTask = tsid.TaskSE3Equality(\n",
+ " \"task-waist\", robot, \"root_joint\"\n",
+ ") # waist -> root_joint\n",
+ "waistTask.setKp(kp_waist * np.ones(6)) # Proportional gain defined before = 500\n",
+ "waistTask.setKd(2.0 * np.sqrt(kp_waist) * np.ones(6)) # Derivative gain = 2 * sqrt(500)\n",
"\n",
"# Add a Mask to the task which will select the vector dimensions on which the task will act.\n",
"# In this case the waist configuration is a vector 6d (position and orientation -> SE3)\n",
"# Here we set a mask = [0 0 0 1 1 1] so the task on the waist will act on the orientation of the robot\n",
"mask = np.ones(6)\n",
- "mask[:3] = 0.\n",
+ "mask[:3] = 0.0\n",
"waistTask.setMask(mask)\n",
"# Add the task to the HQP with weight = 1.0, priority level = 1 (in the cost function) and a transition duration = 0.0\n",
"invdyn.addMotionTask(waistTask, w_waist, 1, 0.0)\n",
@@ -458,8 +497,10 @@
"\n",
"# POSTURE Task\n",
"postureTask = tsid.TaskJointPosture(\"task-posture\", robot)\n",
- "postureTask.setKp(kp_posture) # Proportional gain defined before (different for each joints)\n",
- "postureTask.setKd(2.0 * kp_posture) # Derivative gain = 2 * kp\n",
+ "postureTask.setKp(\n",
+ " kp_posture\n",
+ ") # Proportional gain defined before (different for each joints)\n",
+ "postureTask.setKd(2.0 * kp_posture) # Derivative gain = 2 * kp\n",
"# Add the task with weight = 0.1, priority level = 1 (in cost function) and a transition duration = 0.0\n",
"invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)"
]
@@ -549,9 +590,13 @@
"# the friction parameter with the ground (mu = 0.3), the normal force bounds (fMin =1.0, fMax=1000.0)\n",
"\n",
"# Right Foot\n",
- "contactRF = tsid.Contact6d(\"contact_rfoot\", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)\n",
- "contactRF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30\n",
- "contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.ones(6)) # Derivative gain = 2 * sqrt(30)\n",
+ "contactRF = tsid.Contact6d(\n",
+ " \"contact_rfoot\", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax\n",
+ ")\n",
+ "contactRF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30\n",
+ "contactRF.setKd(\n",
+ " 2.0 * np.sqrt(kp_contact) * np.ones(6)\n",
+ ") # Derivative gain = 2 * sqrt(30)\n",
"# Reference position of the right ankle -> initial position\n",
"H_rf_ref = robot.position(data, model.getJointId(rf_frame_name))\n",
"contactRF.setReference(H_rf_ref)\n",
@@ -560,9 +605,13 @@
"invdyn.addRigidContact(contactRF, w_forceRef)\n",
"\n",
"# Left Foot\n",
- "contactLF = tsid.Contact6d(\"contact_lfoot\", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax)\n",
- "contactLF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30\n",
- "contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.ones(6)) # Derivative gain = 2 * sqrt(30)\n",
+ "contactLF = tsid.Contact6d(\n",
+ " \"contact_lfoot\", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax\n",
+ ")\n",
+ "contactLF.setKp(kp_contact * np.ones(6)) # Proportional gain defined before = 30\n",
+ "contactLF.setKd(\n",
+ " 2.0 * np.sqrt(kp_contact) * np.ones(6)\n",
+ ") # Derivative gain = 2 * sqrt(30)\n",
"# Reference position of the left ankle -> initial position\n",
"H_lf_ref = robot.position(data, model.getJointId(lf_frame_name))\n",
"contactLF.setReference(H_lf_ref)\n",
@@ -594,14 +643,20 @@
"source": [
"# Set the reference trajectory of the tasks\n",
"\n",
- "com_ref = data.com[0] # Initial value of the CoM\n",
+ "com_ref = data.com[0] # Initial value of the CoM\n",
"trajCom = tsid.TrajectoryEuclidianConstant(\"traj_com\", com_ref)\n",
- "sampleCom = trajCom.computeNext() # Compute the first step of the trajectory from the initial value\n",
+ "sampleCom = (\n",
+ " trajCom.computeNext()\n",
+ ") # Compute the first step of the trajectory from the initial value\n",
"\n",
- "q_ref = q[7:] # Initial value of the joints of the robot (in halfSitting position without the freeFlyer (6 first values))\n",
+ "q_ref = q[\n",
+ " 7:\n",
+ "] # Initial value of the joints of the robot (in halfSitting position without the freeFlyer (6 first values))\n",
"trajPosture = tsid.TrajectoryEuclidianConstant(\"traj_joint\", q_ref)\n",
"\n",
- "waist_ref = robot.position(data, model.getJointId('root_joint')) # Initial value of the waist (root_joint)\n",
+ "waist_ref = robot.position(\n",
+ " data, model.getJointId(\"root_joint\")\n",
+ ") # Initial value of the waist (root_joint)\n",
"# Here the waist is defined as a 6d vector (position + orientation) so it is in the SE3 group (Lie group)\n",
"# Thus, the trajectory is not Euclidian but remains in the SE3 domain -> TrajectorySE3Constant\n",
"trajWaist = tsid.TrajectorySE3Constant(\"traj_waist\", waist_ref)"
@@ -617,7 +672,7 @@
"# Use EiquadprogFast: dynamic matrix sizes (memory allocation performed only when resizing)\n",
"solver = tsid.SolverHQuadProgFast(\"qp solver\")\n",
"# Resize the solver to fit the number of variables, equality and inequality constraints\n",
- "solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn) "
+ "solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)"
]
},
{
@@ -635,7 +690,7 @@
"com_pos_ref = np.full((3, N_SIMULATION), np.nan)\n",
"com_vel_ref = np.full((3, N_SIMULATION), np.nan)\n",
"com_acc_ref = np.full((3, N_SIMULATION), np.nan)\n",
- "com_acc_des = np.full((3, N_SIMULATION), np.nan) "
+ "com_acc_des = np.full((3, N_SIMULATION), np.nan)"
]
},
{
@@ -646,11 +701,15 @@
"source": [
"# Parametes of the CoM sinusoid\n",
"\n",
- "offset = robot.com(data) # offset of the measured CoM\n",
+ "offset = robot.com(data) # offset of the measured CoM\n",
"amp = np.array([0.0, 0.05, 0.0]) # amplitude function of 0.05 along the y axis\n",
- "two_pi_f = 2 * np.pi * np.array([0.0, 0.5, 0.0]) # 2Ï€ function along the y axis with 0.5 amplitude\n",
+ "two_pi_f = (\n",
+ " 2 * np.pi * np.array([0.0, 0.5, 0.0])\n",
+ ") # 2Ï€ function along the y axis with 0.5 amplitude\n",
"two_pi_f_amp = two_pi_f * amp # 2Ï€ function times amplitude function\n",
- "two_pi_f_squared_amp = two_pi_f * two_pi_f_amp # 2Ï€ function times squared amplitude function"
+ "two_pi_f_squared_amp = (\n",
+ " two_pi_f * two_pi_f_amp\n",
+ ") # 2Ï€ function times squared amplitude function"
]
},
{
@@ -707,25 +766,34 @@
" tau = invdyn.getActuatorForces(sol)\n",
" dv = invdyn.getAccelerations(sol)\n",
"\n",
- " com_pos[:,i] = robot.com(invdyn.data())\n",
- " com_vel[:,i] = robot.com_vel(invdyn.data())\n",
- " com_acc[:,i] = comTask.getAcceleration(dv)\n",
- " com_pos_ref[:,i] = sampleCom.pos()\n",
- " com_vel_ref[:,i] = sampleCom.vel()\n",
- " com_acc_ref[:,i] = sampleCom.acc()\n",
- " com_acc_des[:,i] = comTask.getDesiredAcceleration\n",
+ " com_pos[:, i] = robot.com(invdyn.data())\n",
+ " com_vel[:, i] = robot.com_vel(invdyn.data())\n",
+ " com_acc[:, i] = comTask.getAcceleration(dv)\n",
+ " com_pos_ref[:, i] = sampleCom.pos()\n",
+ " com_vel_ref[:, i] = sampleCom.vel()\n",
+ " com_acc_ref[:, i] = sampleCom.acc()\n",
+ " com_acc_des[:, i] = comTask.getDesiredAcceleration\n",
"\n",
" if i % PRINT_N == 0:\n",
" print(\"Time %.3f\" % t)\n",
" if invdyn.checkContact(contactRF.name, sol):\n",
" f = invdyn.getContactForce(contactRF.name, sol)\n",
- " print(\"\\tnormal force %s: %.1f\" % (contactRF.name.ljust(20, '.'), contactRF.getNormalForce(f)))\n",
+ " print(\n",
+ " \"\\tnormal force %s: %.1f\"\n",
+ " % (contactRF.name.ljust(20, \".\"), contactRF.getNormalForce(f))\n",
+ " )\n",
"\n",
" if invdyn.checkContact(contactLF.name, sol):\n",
" f = invdyn.getContactForce(contactLF.name, sol)\n",
- " print(\"\\tnormal force %s: %.1f\" % (contactLF.name.ljust(20, '.'), contactLF.getNormalForce(f)))\n",
- "\n",
- " print(\"\\ttracking err %s: %.3f\" % (comTask.name.ljust(20, '.'), norm(comTask.position_error, 2)))\n",
+ " print(\n",
+ " \"\\tnormal force %s: %.1f\"\n",
+ " % (contactLF.name.ljust(20, \".\"), contactLF.getNormalForce(f))\n",
+ " )\n",
+ "\n",
+ " print(\n",
+ " \"\\ttracking err %s: %.3f\"\n",
+ " % (comTask.name.ljust(20, \".\"), norm(comTask.position_error, 2))\n",
+ " )\n",
" print(\"\\t||v||: %.3f\\t ||dv||: %.3f\" % (norm(v, 2), norm(dv)))\n",
"\n",
" v_mean = v + 0.5 * dt * dv\n",
@@ -737,7 +805,7 @@
" viz.display(q)\n",
"\n",
" time_spent = tmp.time() - time_start\n",
- " if time_spent < dt: \n",
+ " if time_spent < dt:\n",
" tmp.sleep(dt - time_spent)"
]
},
@@ -761,10 +829,10 @@
"\n",
"(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
"for i in range(3):\n",
- " ax[i].plot(time, com_pos[i,:], label='CoM %i' % i)\n",
- " ax[i].plot(time, com_pos_ref[i,:], 'r:', label='CoM Ref %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('CoM [m]')\n",
+ " ax[i].plot(time, com_pos[i, :], label=\"CoM %i\" % i)\n",
+ " ax[i].plot(time, com_pos_ref[i, :], \"r:\", label=\"CoM Ref %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"CoM [m]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"\n",
@@ -781,10 +849,10 @@
"\n",
"(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
"for i in range(3):\n",
- " ax[i].plot(time, com_vel[i,:], label='CoM Vel %i' % i)\n",
- " ax[i].plot(time, com_vel_ref[i,:], 'r:', label='CoM Vel Ref %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('CoM Vel [m/s]')\n",
+ " ax[i].plot(time, com_vel[i, :], label=\"CoM Vel %i\" % i)\n",
+ " ax[i].plot(time, com_vel_ref[i, :], \"r:\", label=\"CoM Vel Ref %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"CoM Vel [m/s]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"\n",
@@ -803,11 +871,11 @@
"\n",
"(f, ax) = plut.create_empty_figure(3, 1, figsize=(10, 10))\n",
"for i in range(3):\n",
- " ax[i].plot(time, com_acc[i,:], label='CoM Acc %i' % i)\n",
- " ax[i].plot(time, com_acc_ref[i,:], 'r:', label='CoM Acc Ref %i' % i)\n",
- " ax[i].plot(time, com_acc_des[i,:], 'g--', label='CoM Acc Des %i' % i)\n",
- " ax[i].set_xlabel('Time [s]')\n",
- " ax[i].set_ylabel('CoM Acc [m/s^2]')\n",
+ " ax[i].plot(time, com_acc[i, :], label=\"CoM Acc %i\" % i)\n",
+ " ax[i].plot(time, com_acc_ref[i, :], \"r:\", label=\"CoM Acc Ref %i\" % i)\n",
+ " ax[i].plot(time, com_acc_des[i, :], \"g--\", label=\"CoM Acc Des %i\" % i)\n",
+ " ax[i].set_xlabel(\"Time [s]\")\n",
+ " ax[i].set_ylabel(\"CoM Acc [m/s^2]\")\n",
" leg = ax[i].legend()\n",
" leg.get_frame().set_alpha(0.5)\n",
"\n",
diff --git a/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb b/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
index c63fad6..1afb26b 100644
--- a/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
+++ b/exercizes/notebooks/ex_3_biped_balance_with_gui.ipynb
@@ -1,5 +1,5 @@
{
- "cells": [
+ "cells": [
{
"cell_type": "code",
"execution_count": null,
@@ -10,13 +10,12 @@
"import time\n",
"import sys\n",
"\n",
- "from IPython.display import display\n",
"from ipywidgets import interact\n",
"import ipywidgets as widgets\n",
"import numpy as np\n",
"import pinocchio as pin\n",
"\n",
- "sys.path.append('..')\n",
+ "sys.path.append(\"..\")\n",
"import talos_conf as conf\n",
"import vizutils\n",
"from tsid_biped import TsidBiped"
@@ -32,15 +31,27 @@
"tsid.q0[2] = 1.02127\n",
"\n",
"com_0 = tsid.robot.com(tsid.formulation.data())\n",
- "H_rf_0 = tsid.robot.framePosition(tsid.formulation.data(), tsid.model.getFrameId(conf.rf_frame_name))\n",
- "H_lf_0 = tsid.robot.framePosition(tsid.formulation.data(), tsid.model.getFrameId(conf.lf_frame_name))\n",
- "\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/com', conf.SPHERE_RADIUS, conf.COM_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/com_ref', conf.REF_SPHERE_RADIUS, conf.COM_REF_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/rf', conf.SPHERE_RADIUS, conf.RF_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/rf_ref', conf.REF_SPHERE_RADIUS, conf.RF_REF_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/lf', conf.SPHERE_RADIUS, conf.LF_SPHERE_COLOR)\n",
- "vizutils.addViewerSphere(tsid.viz, 'world/lf_ref', conf.REF_SPHERE_RADIUS, conf.LF_REF_SPHERE_COLOR)"
+ "H_rf_0 = tsid.robot.framePosition(\n",
+ " tsid.formulation.data(), tsid.model.getFrameId(conf.rf_frame_name)\n",
+ ")\n",
+ "H_lf_0 = tsid.robot.framePosition(\n",
+ " tsid.formulation.data(), tsid.model.getFrameId(conf.lf_frame_name)\n",
+ ")\n",
+ "\n",
+ "vizutils.addViewerSphere(\n",
+ " tsid.viz, \"world/com\", conf.SPHERE_RADIUS, conf.COM_SPHERE_COLOR\n",
+ ")\n",
+ "vizutils.addViewerSphere(\n",
+ " tsid.viz, \"world/com_ref\", conf.REF_SPHERE_RADIUS, conf.COM_REF_SPHERE_COLOR\n",
+ ")\n",
+ "vizutils.addViewerSphere(tsid.viz, \"world/rf\", conf.SPHERE_RADIUS, conf.RF_SPHERE_COLOR)\n",
+ "vizutils.addViewerSphere(\n",
+ " tsid.viz, \"world/rf_ref\", conf.REF_SPHERE_RADIUS, conf.RF_REF_SPHERE_COLOR\n",
+ ")\n",
+ "vizutils.addViewerSphere(tsid.viz, \"world/lf\", conf.SPHERE_RADIUS, conf.LF_SPHERE_COLOR)\n",
+ "vizutils.addViewerSphere(\n",
+ " tsid.viz, \"world/lf_ref\", conf.REF_SPHERE_RADIUS, conf.LF_REF_SPHERE_COLOR\n",
+ ")"
]
},
{
@@ -109,19 +120,31 @@
" H_rf = tsid.robot.framePosition(tsid.formulation.data(), tsid.RF)\n",
" x_lf_ref = tsid.trajLF.getSample(t).pos()[:3]\n",
" x_rf_ref = tsid.trajRF.getSample(t).pos()[:3]\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/com', x_com.tolist() + [0, 0, 0, 1.])\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/com_ref', x_com_ref.tolist() + [0, 0, 0, 1.])\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/rf', pin.SE3ToXYZQUATtuple(H_rf))\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/lf', pin.SE3ToXYZQUATtuple(H_lf))\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/rf_ref', x_rf_ref.tolist() + [0, 0, 0, 1.])\n",
- " vizutils.applyViewerConfiguration(tsid.viz, 'world/lf_ref', x_lf_ref.tolist() + [0, 0, 0, 1.])\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/com\", x_com.tolist() + [0, 0, 0, 1.0]\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/com_ref\", x_com_ref.tolist() + [0, 0, 0, 1.0]\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/rf\", pin.SE3ToXYZQUATtuple(H_rf)\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/lf\", pin.SE3ToXYZQUATtuple(H_lf)\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/rf_ref\", x_rf_ref.tolist() + [0, 0, 0, 1.0]\n",
+ " )\n",
+ " vizutils.applyViewerConfiguration(\n",
+ " tsid.viz, \"world/lf_ref\", x_lf_ref.tolist() + [0, 0, 0, 1.0]\n",
+ " )\n",
"\n",
" time_spent = time.time() - time_start\n",
" time_avg = (i * time_avg + time_spent) / (i + 1)\n",
"\n",
" if time_avg < 0.9 * conf.dt:\n",
" time.sleep(conf.dt - time_avg)\n",
- " print('end of simulation')"
+ " print(\"end of simulation\")"
]
},
{
@@ -136,41 +159,44 @@
"th_simu.start()\n",
"\n",
"\n",
- "@interact(com_x=(-10., 10.), com_y=(-15., 15.), com_z=(-40., 40.))\n",
+ "@interact(com_x=(-10.0, 10.0), com_y=(-15.0, 15.0), com_z=(-40.0, 40.0))\n",
"def sliders_com(com_x, com_y, com_z):\n",
- " tsid.trajCom.setReference(com_0 + np.array([1e-2 * com_x, 1e-2 * com_y, 1e-2 * com_z]).T)\n",
+ " tsid.trajCom.setReference(\n",
+ " com_0 + np.array([1e-2 * com_x, 1e-2 * com_y, 1e-2 * com_z]).T\n",
+ " )\n",
"\n",
"\n",
- "@interact(rf_x=(-30., 30.), rf_y=(-30., 30.), rf_z=(-30., 30.))\n",
+ "@interact(rf_x=(-30.0, 30.0), rf_y=(-30.0, 30.0), rf_z=(-30.0, 30.0))\n",
"def sliders_rf(rf_x, rf_y, rf_z):\n",
" H_rf_ref = H_rf_0.copy()\n",
" H_rf_ref.translation += np.array([1e-2 * rf_x, 1e-2 * rf_y, 1e-2 * rf_z]).T\n",
" tsid.trajRF.setReference(H_rf_ref)\n",
"\n",
"\n",
- "@interact(lf_x=(-30., 30.), lf_y=(-30., 30.), lf_z=(-30., 30.))\n",
+ "@interact(lf_x=(-30.0, 30.0), lf_y=(-30.0, 30.0), lf_z=(-30.0, 30.0))\n",
"def sliders_lf(lf_x, lf_y, lf_z):\n",
" H_lf_ref = H_lf_0.copy()\n",
- " H_lf_ref.translation += + np.array([1e-2 * lf_x, 1e-2 * lf_y, 1e-2 * lf_z]).T\n",
+ " H_lf_ref.translation += +np.array([1e-2 * lf_x, 1e-2 * lf_y, 1e-2 * lf_z]).T\n",
" tsid.trajLF.setReference(H_lf_ref)\n",
"\n",
"\n",
"def buttons(b):\n",
- " if b.description == 'Stop':\n",
+ " if b.description == \"Stop\":\n",
" run.clear()\n",
" th_simu.join()\n",
- " elif b.description == 'Switch Right Foot':\n",
+ " elif b.description == \"Switch Right Foot\":\n",
" if tsid.contact_RF_active:\n",
" tsid.remove_contact_RF()\n",
" else:\n",
" tsid.add_contact_RF()\n",
- " elif b.description == 'Switch Left Foot':\n",
+ " elif b.description == \"Switch Left Foot\":\n",
" if tsid.contact_LF_active:\n",
" tsid.remove_contact_LF()\n",
" else:\n",
- " tsid.add_contact_LF() \n",
+ " tsid.add_contact_LF()\n",
"\n",
- "for action in ['Stop', 'Switch Right Foot', 'Switch Left Foot']:\n",
+ "\n",
+ "for action in [\"Stop\", \"Switch Right Foot\", \"Switch Left Foot\"]:\n",
" button = widgets.Button(description=action)\n",
" button.on_click(buttons)\n",
" output.append_display_data(button)"
@@ -182,7 +208,7 @@
"metadata": {},
"outputs": [],
"source": [
- "hasattr(tsid.viz.viewer, 'jupyter_cell') and tsid.viz.viewer.jupyter_cell()"
+ "hasattr(tsid.viz.viewer, \"jupyter_cell\") and tsid.viz.viewer.jupyter_cell()"
]
},
{
diff --git a/exercizes/plot_utils.py b/exercizes/plot_utils.py
index 0a44527..29eae2b 100644
--- a/exercizes/plot_utils.py
+++ b/exercizes/plot_utils.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Fri Jan 16 09:16:56 2015
+"""Created on Fri Jan 16 09:16:56 2015
@author: adelpret
"""
diff --git a/exercizes/romeo_conf.py b/exercizes/romeo_conf.py
index a204015..9ac83eb 100644
--- a/exercizes/romeo_conf.py
+++ b/exercizes/romeo_conf.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
diff --git a/exercizes/test_cop_task.py b/exercizes/test_cop_task.py
index 6300c45..be48ed3 100644
--- a/exercizes/test_cop_task.py
+++ b/exercizes/test_cop_task.py
@@ -1,10 +1,10 @@
-""" This script is a slight variation of ex_4_walking.py introduced to test the new
- center of pressure (CoP) task. In this script, besides tracking a reference
- center of mass (CoM), the TSID controller also tries to track a reference CoP.
- The resulting motion doesn't look great because the reference CoP is an open-loop
- reference trajectory, so it is not stabilizing, and it conflicts with the CoM task.
- However, the results show that the CoP is tracked reasonably well during the motion,
- which was the goal of the test, validating the CoP task.
+"""This script is a slight variation of ex_4_walking.py introduced to test the new
+center of pressure (CoP) task. In this script, besides tracking a reference
+center of mass (CoM), the TSID controller also tries to track a reference CoP.
+The resulting motion doesn't look great because the reference CoP is an open-loop
+reference trajectory, so it is not stabilizing, and it conflicts with the CoM task.
+However, the results show that the CoP is tracked reasonably well during the motion,
+which was the goal of the test, validating the CoP task.
"""
import time
diff --git a/exercizes/ur5_conf.py b/exercizes/ur5_conf.py
index 106e88d..cdc1a61 100644
--- a/exercizes/ur5_conf.py
+++ b/exercizes/ur5_conf.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
diff --git a/exercizes/ur5_reaching_conf.py b/exercizes/ur5_reaching_conf.py
index ae2279e..e8bb731 100644
--- a/exercizes/ur5_reaching_conf.py
+++ b/exercizes/ur5_reaching_conf.py
@@ -1,6 +1,4 @@
-# -*- coding: utf-8 -*-
-"""
-Created on Thu Apr 18 09:47:07 2019
+"""Created on Thu Apr 18 09:47:07 2019
@author: student
"""
diff --git a/tests/python/generator.py b/tests/python/generator.py
index 5aea6b5..4950c63 100644
--- a/tests/python/generator.py
+++ b/tests/python/generator.py
@@ -6,8 +6,7 @@ import pinocchio as pin
def create_7dof_arm(revoluteOnly=False):
- """
- Create a 7 DoF robot arm (with spherical joint for shoulder and wrist and revolute
+ """Create a 7 DoF robot arm (with spherical joint for shoulder and wrist and revolute
joint for elbow)
Optionnal parameters:
@@ -15,8 +14,10 @@ def create_7dof_arm(revoluteOnly=False):
joints. (Spherical joints are replaced by 3 revolute joints)
Return:
+ ------
model: pinocchio model of the robot
geom_model: pinocchio geometric model of the robot
+
"""
# Some useful values
geom_radius = 0.1 # radius of the arm geometries
--
GitLab