diff --git a/README.md b/README.md index 02d87c8..7e6e099 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ To install you can use the following commands ``` -conda create -n comododev -c conda-forge adam-robotics idyntree bipedal-locomotion-framework=0.15.0 mujoco mujoco-python numpy mujoco-python-viewer matplotlib urllib3 urchin +conda create -n comododev -c conda-forge adam-robotics idyntree bipedal-locomotion-framework=0.16.0 mujoco mujoco-python numpy mujoco-python-viewer matplotlib urllib3 urchin resolve-robotics-uri-py conda activate comododev pip install --no-deps git+https://github.com/CarlottaSartore/urdf-modifiers.git@scalar_modification pip install --no-deps -e . diff --git a/examples/mujoco_walking.ipynb b/examples/mujoco_walking.ipynb index cf3b743..044ced0 100644 --- a/examples/mujoco_walking.ipynb +++ b/examples/mujoco_walking.ipynb @@ -4,7 +4,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "## Comodo Exmaple \n", + "## Mujoco with TSID and MPC Example \n", "This examples, load a basic robot model (i.e. composed only of basic shapes), modifies the links of such a robot model by elongating the legs, define instances of the TSID (Task Based Inverse Dynamics) and Centroidal MPC controller and simulate the behavior of the robot using mujoco. " ] }, @@ -21,7 +21,7 @@ "from comodo.centroidalMPC.centroidalMPC import CentroidalMPC\n", "from comodo.centroidalMPC.mpcParameterTuning import MPCParameterTuning\n", "from comodo.TSIDController.TSIDParameterTuning import TSIDParameterTuning\n", - "from comodo.TSIDController.TSIDController import TSIDController " + "from comodo.TSIDController.TSIDController import TSIDController" ] }, { @@ -30,7 +30,7 @@ "metadata": {}, "outputs": [], "source": [ - "# General import \n", + "# General import\n", "import xml.etree.ElementTree as ET\n", "import numpy as np\n", "import tempfile\n", @@ -43,9 +43,9 @@ "metadata": {}, "outputs": [], "source": [ - "# Getting stickbot urdf file and convert it to string \n", + "# Getting stickbot urdf file and convert it to string\n", "urdf_robot_file = tempfile.NamedTemporaryFile(mode=\"w+\")\n", - "url = 'https://raw.githubusercontent.com/icub-tech-iit/ergocub-gazebo-simulations/master/models/stickBot/model.urdf'\n", + "url = \"https://raw.githubusercontent.com/icub-tech-iit/ergocub-gazebo-simulations/master/models/stickBot/model.urdf\"\n", "urllib.request.urlretrieve(url, urdf_robot_file.name)\n", "# Load the URDF file\n", "tree = ET.parse(urdf_robot_file.name)\n", @@ -65,7 +65,7 @@ "metadata": {}, "outputs": [], "source": [ - "# Define parametric links and controlled joints \n", + "# Define parametric links and controlled joints\n", "legs_link_names = [\"hip_3\", \"lower_leg\"]\n", "joint_name_list = [\n", " \"r_shoulder_pitch\",\n", @@ -104,11 +104,11 @@ " right_leg_item = \"r_\" + item\n", " modifications.update({left_leg_item: 1.2})\n", " modifications.update({right_leg_item: 1.2})\n", - "# Motors Parameters \n", - "Im_arms = 1e-3*np.ones(4) # from 0-4\n", - "Im_legs = 1e-3*np.ones(6) # from 5-10\n", - "kv_arms = 0.001*np.ones(4) # from 11-14\n", - "kv_legs = 0.001*np.ones(6) #from 20\n", + "# Motors Parameters\n", + "Im_arms = 1e-3 * np.ones(4) # from 0-4\n", + "Im_legs = 1e-3 * np.ones(6) # from 5-10\n", + "kv_arms = 0.001 * np.ones(4) # from 11-14\n", + "kv_legs = 0.001 * np.ones(6) # from 20\n", "\n", "Im = np.concatenate((Im_arms, Im_arms, Im_legs, Im_legs))\n", "kv = np.concatenate((kv_arms, kv_arms, kv_legs, kv_legs))" @@ -136,7 +136,9 @@ "source": [ "# Define simulator and set initial position\n", "mujoco_instance = MujocoSimulator()\n", - "mujoco_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im)\n", + "mujoco_instance.load_model(\n", + " robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im\n", + ")\n", "s, ds, tau = mujoco_instance.get_state()\n", "t = mujoco_instance.get_simulation_time()\n", "H_b = mujoco_instance.get_base()\n", @@ -155,7 +157,7 @@ "tsid_parameter = TSIDParameterTuning()\n", "mpc_parameters = MPCParameterTuning()\n", "\n", - "# TSID Instance \n", + "# TSID Instance\n", "TSID_controller_instance = TSIDController(frequency=0.01, robot_model=robot_model_init)\n", "TSID_controller_instance.define_tasks(tsid_parameter)\n", "TSID_controller_instance.set_state_with_base(s, ds, H_b, w_b, t)\n", @@ -170,10 +172,10 @@ "TSID_controller_instance.compute_com_position()\n", "mpc.define_test_com_traj(TSID_controller_instance.COM.toNumPy())\n", "\n", - "# Set initial robot state and plan trajectories \n", + "# Set initial robot state and plan trajectories\n", "mujoco_instance.step(1)\n", "\n", - "# Reading the state \n", + "# Reading the state\n", "s, ds, tau = mujoco_instance.get_state()\n", "H_b = mujoco_instance.get_base()\n", "w_b = mujoco_instance.get_base_velocity()\n", @@ -182,7 +184,7 @@ "# MPC\n", "mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", "mpc.initialize_centroidal_integrator(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", - "mpc_output = mpc.plan_trajectory()\n" + "mpc_output = mpc.plan_trajectory()" ] }, { @@ -191,7 +193,7 @@ "metadata": {}, "outputs": [], "source": [ - "# Set loop variables \n", + "# Set loop variables\n", "TIME_TH = 20\n", "\n", "# Define number of steps\n", @@ -212,20 +214,19 @@ "metadata": {}, "outputs": [], "source": [ - "# Simulation-control loop \n", + "# Simulation-control loop\n", "while t < TIME_TH:\n", - "\n", " # Reading robot state from simulator\n", " s, ds, tau = mujoco_instance.get_state()\n", " energy_i = np.linalg.norm(tau)\n", " H_b = mujoco_instance.get_base()\n", " w_b = mujoco_instance.get_base_velocity()\n", " t = mujoco_instance.get_simulation_time()\n", - " \n", + "\n", " # Update TSID\n", " TSID_controller_instance.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", - " \n", - " # MPC plan \n", + "\n", + " # MPC plan\n", " if counter == 0:\n", " mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", " mpc.update_references()\n", @@ -236,10 +237,10 @@ " break\n", "\n", " # Reading new references\n", - " com, dcom, forces_left, forces_right = mpc.get_references()\n", + " com, dcom, forces_left, forces_right, ang_mom = mpc.get_references()\n", " left_foot, right_foot = mpc.contact_planner.get_references_swing_foot_planner()\n", - " \n", - " # Update references TSID \n", + "\n", + " # Update references TSID\n", " TSID_controller_instance.update_task_references_mpc(\n", " com=com,\n", " dcom=dcom,\n", @@ -251,7 +252,7 @@ " wrenches_right=forces_right,\n", " )\n", "\n", - " # Run control \n", + " # Run control\n", " succeded_controller = TSID_controller_instance.run()\n", "\n", " if not (succeded_controller):\n", @@ -259,7 +260,7 @@ " break\n", "\n", " tau = TSID_controller_instance.get_torque()\n", - " \n", + "\n", " # Step the simulator\n", " mujoco_instance.set_input(tau)\n", " mujoco_instance.step_with_motors(n_step=n_step, torque=tau)\n", diff --git a/examples/mujoco_walking_position.ipynb b/examples/mujoco_walking_position.ipynb new file mode 100644 index 0000000..88b43b2 --- /dev/null +++ b/examples/mujoco_walking_position.ipynb @@ -0,0 +1,321 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## (W.I.P.)Mujoco with IK and MPC Example \n", + "This example loads a basic robot model (i.e. composed only of basic shapes), modifies the links of such a robot model by elongating the legs, defines instances of the IK (Inverse Kinematic) and Centroidal MPC controller, and simulates the behavior of the robot using mujoco. \n", + "\n", + ":warning: this is still a work in progress. Missing tuning on the controller to perform few steps " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Comodo import\n", + "from comodo.mujocoSimulator.mujocoSimulator import MujocoSimulator\n", + "from comodo.robotModel.robotModel import RobotModel\n", + "from comodo.robotModel.createUrdf import createUrdf\n", + "from comodo.centroidalMPC.centroidalMPC import CentroidalMPC\n", + "from comodo.centroidalMPC.mpcParameterTuning import MPCParameterTuning\n", + "from comodo.inverseKinematics.inverseKinematicsParamTuning import (\n", + " InverseKinematicsParamTuning,\n", + ")\n", + "from comodo.inverseKinematics.inverseKinematics import InverseKinematics" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# General import\n", + "import xml.etree.ElementTree as ET\n", + "import numpy as np\n", + "import tempfile\n", + "import urllib.request" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Getting stickbot urdf file and convert it to string\n", + "urdf_robot_file = tempfile.NamedTemporaryFile(mode=\"w+\")\n", + "url = \"https://raw.githubusercontent.com/icub-tech-iit/ergocub-gazebo-simulations/master/models/stickBot/model.urdf\"\n", + "urllib.request.urlretrieve(url, urdf_robot_file.name)\n", + "# Load the URDF file\n", + "tree = ET.parse(urdf_robot_file.name)\n", + "root = tree.getroot()\n", + "\n", + "# Convert the XML tree to a string\n", + "robot_urdf_string_original = ET.tostring(root)\n", + "\n", + "create_urdf_instance = createUrdf(\n", + " original_urdf_path=urdf_robot_file.name, save_gazebo_plugin=False\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define parametric links and controlled joints\n", + "legs_link_names = [\"hip_3\", \"lower_leg\"]\n", + "joint_name_list = [\n", + " \"r_shoulder_pitch\",\n", + " \"r_shoulder_roll\",\n", + " \"r_shoulder_yaw\",\n", + " \"r_elbow\",\n", + " \"l_shoulder_pitch\",\n", + " \"l_shoulder_roll\",\n", + " \"l_shoulder_yaw\",\n", + " \"l_elbow\",\n", + " \"r_hip_pitch\",\n", + " \"r_hip_roll\",\n", + " \"r_hip_yaw\",\n", + " \"r_knee\",\n", + " \"r_ankle_pitch\",\n", + " \"r_ankle_roll\",\n", + " \"l_hip_pitch\",\n", + " \"l_hip_roll\",\n", + " \"l_hip_yaw\",\n", + " \"l_knee\",\n", + " \"l_ankle_pitch\",\n", + " \"l_ankle_roll\",\n", + "]" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define the robot modifications\n", + "modifications = {}\n", + "for item in legs_link_names:\n", + " left_leg_item = \"l_\" + item\n", + " right_leg_item = \"r_\" + item\n", + " modifications.update({left_leg_item: 1.2})\n", + " modifications.update({right_leg_item: 1.2})\n", + "# Motors Parameters\n", + "Im_arms = 1e-3 * np.ones(4) # from 0-4\n", + "Im_legs = 1e-3 * np.ones(6) # from 5-10\n", + "kv_arms = 0.001 * np.ones(4) # from 11-14\n", + "kv_legs = 0.001 * np.ones(6) # from 20\n", + "\n", + "Im = np.concatenate((Im_arms, Im_arms, Im_legs, Im_legs))\n", + "kv = np.concatenate((kv_arms, kv_arms, kv_legs, kv_legs))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Modify the robot model and initialize\n", + "create_urdf_instance.modify_lengths(modifications)\n", + "urdf_robot_string = create_urdf_instance.write_urdf_to_file()\n", + "create_urdf_instance.reset_modifications()\n", + "robot_model_init = RobotModel(urdf_robot_string, \"stickBot\", joint_name_list)\n", + "s_des, xyz_rpy, H_b = robot_model_init.compute_desired_position_walking()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define simulator and set initial position\n", + "mujoco_instance = MujocoSimulator()\n", + "mujoco_instance.load_model(\n", + " robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im\n", + ")\n", + "s, ds, tau = mujoco_instance.get_state()\n", + "t = mujoco_instance.get_simulation_time()\n", + "H_b = mujoco_instance.get_base()\n", + "w_b = mujoco_instance.get_base_velocity()\n", + "mujoco_instance.set_visualize_robot_flag(True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Define the controller parameters and instantiate the controller\n", + "# Controller Parameters\n", + "ik_parameters = InverseKinematicsParamTuning()\n", + "mpc_parameters = MPCParameterTuning()\n", + "\n", + "# IK Instance\n", + "IK_controller_instance = InverseKinematics(frequency=0.01, robot_model=robot_model_init)\n", + "IK_controller_instance.define_tasks(parameters=ik_parameters)\n", + "IK_controller_instance.set_state_with_base(s, ds, H_b, w_b, t)\n", + "\n", + "# MPC Instance\n", + "step_length = 0.1\n", + "mpc = CentroidalMPC(robot_model=robot_model_init, step_length=step_length)\n", + "mpc.intialize_mpc(mpc_parameters=mpc_parameters)\n", + "\n", + "# Set desired quantities\n", + "mpc.configure(s_init=s_des, H_b_init=H_b)\n", + "IK_controller_instance.update_com(H_b, s)\n", + "IK_controller_instance.set_desired_base_orientation()\n", + "mpc.define_test_com_traj(IK_controller_instance.com)\n", + "\n", + "# Set initial robot state and plan trajectories\n", + "mujoco_instance.step(1)\n", + "\n", + "# Reading the state\n", + "s, ds, tau = mujoco_instance.get_state()\n", + "H_b = mujoco_instance.get_base()\n", + "w_b = mujoco_instance.get_base_velocity()\n", + "t = mujoco_instance.get_simulation_time()\n", + "\n", + "# MPC\n", + "mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + "mpc.initialize_centroidal_integrator(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + "mpc_output = mpc.plan_trajectory()\n", + "\n", + "# IK Integrator\n", + "IK_controller_instance.define_integrator()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Set loop variables\n", + "TIME_TH = 20\n", + "\n", + "# Define number of steps\n", + "n_step = int(\n", + " IK_controller_instance.frequency / mujoco_instance.get_simulation_frequency()\n", + ")\n", + "n_step_mpc_ik = int(mpc.get_frequency_seconds() / IK_controller_instance.frequency)\n", + "\n", + "counter = 0\n", + "mpc_success = True\n", + "energy_tot = 0.0\n", + "succeded_controller = True" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Simulation-control loop\n", + "while t < TIME_TH:\n", + " # Reading robot state from simulator\n", + " s, ds, tau = mujoco_instance.get_state()\n", + " energy_i = np.linalg.norm(tau)\n", + " H_b = mujoco_instance.get_base()\n", + " w_b = mujoco_instance.get_base_velocity()\n", + " t = mujoco_instance.get_simulation_time()\n", + " if H_b[2, 3] < 0.5:\n", + " robot_standing = False\n", + " print(\"Robot fell\")\n", + " break\n", + "\n", + " # MPC plan\n", + " if counter == 0:\n", + " mpc.set_state_with_base(s=s, s_dot=ds, H_b=H_b, w_b=w_b, t=t)\n", + " mpc.update_references()\n", + " mpc_success = mpc.plan_trajectory()\n", + " mpc.contact_planner.advance_swing_foot_planner()\n", + " if not (mpc_success):\n", + " print(\"MPC failed\")\n", + " break\n", + "\n", + " # Reading new references\n", + " com, dcom, forces_left, forces_right, ang_mom = mpc.get_references()\n", + " left_foot, right_foot = mpc.contact_planner.get_references_swing_foot_planner()\n", + " # Updating integrator\n", + " (\n", + " left_foot_wrench,\n", + " rigth_foot_wrench,\n", + " ) = mujoco_instance.get_feet_wrench()\n", + " IK_controller_instance.compute_zmp(\n", + " left_foot_wrench, \n", + " rigth_foot_wrench\n", + " )\n", + " IK_controller_instance.update_task_references_mpc(\n", + " com=com,\n", + " dcom=dcom,\n", + " ddcom=np.zeros(3),\n", + " left_foot_desired=left_foot,\n", + " right_foot_desired=right_foot,\n", + " s_desired=np.array(s_des),\n", + " wrenches_left=forces_left,\n", + " wrenches_right=forces_right,\n", + " H_omega=ang_mom,\n", + " )\n", + " succeded_controller = IK_controller_instance.run()\n", + " IK_controller_instance.update_state()\n", + " IK_controller_instance.update_com(H_b, s)\n", + "\n", + " if not (succeded_controller):\n", + " print(\"Controller failed\")\n", + " break\n", + "\n", + " s_ctrl = IK_controller_instance.get_output()\n", + "\n", + " # Step the simulator\n", + " mujoco_instance.set_position_input(s_ctrl)\n", + " mujoco_instance.step(n_step=n_step)\n", + " counter = counter + 1\n", + " if counter == n_step_mpc_ik:\n", + " counter = 0" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Closing visualization\n", + "mujoco_instance.close_visualization()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "comodo", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.14" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/src/comodo/centroidalMPC/centroidalMPC.py b/src/comodo/centroidalMPC/centroidalMPC.py index b1eba3b..0298b70 100644 --- a/src/comodo/centroidalMPC/centroidalMPC.py +++ b/src/comodo/centroidalMPC/centroidalMPC.py @@ -10,11 +10,12 @@ class CentroidalMPC(Planner): def __init__(self, robot_model, step_length, frequency_ms=100): - self.dT = timedelta(milliseconds=frequency_ms) self.dT_in_seconds = frequency_ms / 1000 self.contact_planner = FootPositionPlanner( - robot_model=robot_model, dT=self.dT_in_seconds, step_length=step_length + robot_model=robot_model, + dT=timedelta(seconds=self.dT_in_seconds), + step_length=step_length, ) self.centroidal_mpc = blf.reduced_model_controllers.CentroidalMPC() scaling = 1.0 @@ -33,7 +34,6 @@ def get_frequency_seconds(self): return self.dT_in_seconds def plan_trajectory(self): - com = self.kindyn.getCenterOfMassPosition() dcom = self.kindyn.getCenterOfMassVelocity() Jcm = iDynTree.MatrixDynSize(6, 6 + self.robot_model.NDoF) @@ -74,7 +74,10 @@ def intialize_mpc(self, mpc_parameters: MPCParameterTuning): self.mpc_param_handler = blf.parameters_handler.StdParametersHandler() self.mpc_param_handler.set_parameter_datetime("sampling_time", self.dT) self.mpc_param_handler.set_parameter_datetime("time_horizon", time_horizon) - self.mpc_param_handler.set_parameter_float("contact_force_symmetry_weight", 1.0) + self.mpc_param_handler.set_parameter_float( + "contact_force_symmetry_weight", + mpc_parameters.contact_force_symmetry_weight, + ) self.mpc_param_handler.set_parameter_int("verbosity", 0) self.mpc_param_handler.set_parameter_int("number_of_maximum_contacts", 2) self.mpc_param_handler.set_parameter_int("number_of_slices", 1) @@ -271,4 +274,5 @@ def get_references(self): forces_right = forces_right + item.force com = self.centroidal_integrator.get_solution()[0] dcom = self.centroidal_integrator.get_solution()[1] - return com, dcom, forces_left, forces_right + ang_mom = self.centroidal_integrator.get_solution()[2] + return com, dcom, forces_left, forces_right, ang_mom diff --git a/src/comodo/centroidalMPC/mpcParameterTuning.py b/src/comodo/centroidalMPC/mpcParameterTuning.py index 115ddd0..4dc92c5 100644 --- a/src/comodo/centroidalMPC/mpcParameterTuning.py +++ b/src/comodo/centroidalMPC/mpcParameterTuning.py @@ -3,11 +3,11 @@ class MPCParameterTuning: def __init__(self) -> None: - self.com_weight = np.asarray([100, 100, 1000]) - self.contact_position_weight = 1e3 - self.force_rate_change_weight = np.asarray([10.0, 10.0, 10.0]) - self.angular_momentum_weight = 1e5 - self.contact_force_symmetry_weight = 1.0 + self.com_weight = np.asarray([2, 2, 80]) + self.contact_position_weight = 125 + self.force_rate_change_weight = np.asarray([130.0, 20.0, 10.0]) + self.angular_momentum_weight = 125 + self.contact_force_symmetry_weight = 35.0 def set_parameters( self, @@ -17,10 +17,18 @@ def set_parameters( angular_mom_weight, contact_force_symmetry_weight, ): - self.com_weight = com_weight + # Forcing the x and y component to be one equal to the other + self.com_weight = np.asanyarray([com_weight[0], com_weight[0], com_weight[1]]) self.contact_position_weight = contac_position self.force_rate_change_weight = force_rate_change self.angular_momentum_weight = angular_mom_weight + self.contact_force_symmetry_weight = contact_force_symmetry_weight def set_from_xk(self, x_k): - self.set_parameters(x_k[:3], x_k[3], x_k[4:7], x_k[7], x_k[8]) + self.set_parameters( + com_weight=x_k[:2], + contac_position=x_k[2], + force_rate_change=x_k[3:6], + angular_mom_weight=x_k[6], + contact_force_symmetry_weight=x_k[7], + ) diff --git a/src/comodo/inverseKinematics/__init__.py b/src/comodo/inverseKinematics/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/comodo/inverseKinematics/inverseKinematics.py b/src/comodo/inverseKinematics/inverseKinematics.py new file mode 100644 index 0000000..2df4f9f --- /dev/null +++ b/src/comodo/inverseKinematics/inverseKinematics.py @@ -0,0 +1,452 @@ +from comodo.abstractClasses.controller import Controller +from comodo.inverseKinematics.inverseKinematicsParamTuning import ( + InverseKinematicsParamTuning, +) +import bipedal_locomotion_framework as blf +import numpy as np +import idyntree.bindings as iDynTree +import manifpy +import copy +import datetime +from scipy.spatial.transform import Rotation + + +class InverseKinematics(Controller): + def __init__(self, frequency, robot_model) -> None: + self.gravity = iDynTree.Vector3() + self.gravity.zero() + self.gravity.setVal(2, -blf.math.StandardAccelerationOfGravitation) + self.kindyn = robot_model.get_idyntree_kyndyn() + self.com_fun = robot_model.CoM_position_fun() + super().__init__(frequency, robot_model) + + def define_tasks(self, parameters: InverseKinematicsParamTuning): + self.robot_velocity_name = "robotVelocity" + + # # Set the parameters ik + qp_ik_param_handler = blf.parameters_handler.StdParametersHandler() + qp_ik_param_handler.set_parameter_string( + name="robot_velocity_variable_name", value=self.robot_velocity_name + ) + + # Initialize the QP inverse kinematics + self.solver = blf.ik.QPInverseKinematics() + self.solver.initialize(handler=qp_ik_param_handler) + + qp_ik_var_handler = blf.system.VariablesHandler() + qp_ik_var_handler.add_variable("robotVelocity", self.robot_model.NDoF + 6) + + # ## COM + # Set the parameters + com_param_handler = blf.parameters_handler.StdParametersHandler() + com_param_handler.set_parameter_string( + name="robot_velocity_variable_name", value=self.robot_velocity_name + ) + com_param_handler.set_parameter_float( + name="kp_linear", value=parameters.com_linear + ) + com_param_handler.set_parameter_vector_bool( + name="mask", value=[True, True, False] + ) + # # Initialize the task + com_task = blf.ik.CoMTask() + com_task.set_kin_dyn(self.kindyn) + com_task.initialize(param_handler=com_param_handler) + com_var_handler = blf.system.VariablesHandler() + com_var_handler.add_variable( + self.robot_velocity_name, self.robot_model.NDoF + 6 + ) + com_task.set_variables_handler(variables_handler=com_var_handler) + self.solver.add_task(task=com_task, task_name="COM", priority=0) + + # ## Joint tracking + # # Set the parameters + joint_tracking_param_handler = blf.parameters_handler.StdParametersHandler() + joint_tracking_param_handler.set_parameter_string( + name="robot_velocity_variable_name", value=self.robot_velocity_name + ) + joint_tracking_param_handler.set_parameter_vector_float( + name="kp", + value=parameters.kp_joint_tracking * np.ones(self.robot_model.NDoF), + ) + + # # Initialize the task + joint_tracking_task = blf.ik.JointTrackingTask() + joint_tracking_task.set_kin_dyn(self.kindyn) + joint_tracking_task.initialize(param_handler=joint_tracking_param_handler) + joint_tracking_var_handler = blf.system.VariablesHandler() + joint_tracking_var_handler.add_variable( + self.robot_velocity_name, self.robot_model.NDoF + 6 + ) + if isinstance(parameters.weigth_joint, list): + # TODO check the size + weigth_joint = parameters.weigth_joint + else: + weigth_joint = parameters.weigth_joint * np.ones(self.robot_model.NDoF) + self.solver.add_task( + task=joint_tracking_task, + task_name="JOINT_REGULARIZATION", + priority=1, + weight_provider=blf.bindings.system.ConstantWeightProvider(weigth_joint), + ) + + # # Set the parameters CHEST + chest_handler = blf.parameters_handler.StdParametersHandler() + chest_handler.set_parameter_string( + name="robot_velocity_variable_name", value=self.robot_velocity_name + ) + chest_handler.set_parameter_string(name="frame_name", value="chest") + chest_handler.set_parameter_float( + name="kp_angular", value=parameters.chest_angular + ) + # # Initialize the task + chest_task = blf.ik.SO3Task() + chest_task.set_kin_dyn(self.kindyn) + chest_task.initialize(param_handler=chest_handler) + chest_var_handler = blf.system.VariablesHandler() + chest_var_handler.add_variable( + self.robot_velocity_name, self.robot_model.NDoF + 6 + ) + chest_task.set_variables_handler(variables_handler=chest_var_handler) + self.solver.add_task( + task=chest_task, + task_name="CHEST", + priority=1, + weight_provider=blf.bindings.system.ConstantWeightProvider( + [10.0, 10.0, 25.0] + ), + ) + + # # Set the parameters ROOT + root_handler = blf.parameters_handler.StdParametersHandler() + root_handler.set_parameter_string( + name="robot_velocity_variable_name", value=self.robot_velocity_name + ) + root_handler.set_parameter_string(name="frame_name", value="root_link") + root_handler.set_parameter_float(name="kp_linear", value=parameters.root_linear) + root_handler.set_parameter_vector_bool(name="mask", value=[False, False, True]) + + # # Initialize the task + root_task = blf.ik.R3Task() + root_task.set_kin_dyn(self.kindyn) + root_task.initialize(param_handler=root_handler) + root_var_handler = blf.system.VariablesHandler() + root_var_handler.add_variable( + self.robot_velocity_name, self.robot_model.NDoF + 6 + ) + root_task.set_variables_handler(variables_handler=root_var_handler) + self.solver.add_task(task=root_task, task_name="ROOT_TASK", priority=0) + + # ## SE3 LEFT FOOT + # # Set the parameters + left_foot_param_handler = blf.parameters_handler.StdParametersHandler() + left_foot_param_handler.set_parameter_string( + name="robot_velocity_variable_name", value=self.robot_velocity_name + ) + left_foot_param_handler.set_parameter_string(name="frame_name", value="l_sole") + left_foot_param_handler.set_parameter_float( + name="kp_linear", value=parameters.foot_linear + ) + left_foot_param_handler.set_parameter_float( + name="kp_angular", value=parameters.foot_angular + ) + + # # Initialize the task + left_foot_taks = blf.ik.SE3Task() + left_foot_taks.set_kin_dyn(self.kindyn) + left_foot_taks.initialize(param_handler=left_foot_param_handler) + left_foot_var_handler = blf.system.VariablesHandler() + left_foot_var_handler.add_variable( + self.robot_velocity_name, self.robot_model.NDoF + 6 + ) + left_foot_taks.set_variables_handler(variables_handler=left_foot_var_handler) + self.solver.add_task(task=left_foot_taks, task_name="LEFT_FOOT", priority=0) + + # ## SE3 RIGTH FOOT + # # Set the parameters + rigth_foot_param_handler = blf.parameters_handler.StdParametersHandler() + rigth_foot_param_handler.set_parameter_string( + name="robot_velocity_variable_name", value=self.robot_velocity_name + ) + rigth_foot_param_handler.set_parameter_string(name="frame_name", value="r_sole") + rigth_foot_param_handler.set_parameter_float( + name="kp_linear", value=parameters.foot_linear + ) + rigth_foot_param_handler.set_parameter_float( + name="kp_angular", value=parameters.foot_angular + ) + + # # Initialize the task + rigth_foot_taks = blf.ik.SE3Task() + rigth_foot_taks.set_kin_dyn(self.kindyn) + rigth_foot_taks.initialize(param_handler=rigth_foot_param_handler) + rigth_foot_var_handler = blf.system.VariablesHandler() + rigth_foot_var_handler.add_variable( + self.robot_velocity_name, self.robot_model.NDoF + 6 + ) + rigth_foot_taks.set_variables_handler(variables_handler=rigth_foot_var_handler) + self.solver.add_task(task=rigth_foot_taks, task_name="RIGHT_FOOT", priority=0) + + self.solver.finalize(qp_ik_var_handler) + self.define_zmp_controller(parameters=parameters) + + def define_zmp_controller(self, parameters: InverseKinematicsParamTuning): + self.zmp_controller = blf.simplified_model_controllers.CoMZMPController() + controller_param_handler = blf.parameters_handler.StdParametersHandler() + controller_param_handler.set_parameter_vector_float( + "zmp_gain", parameters.zmp_gain + ) + controller_param_handler.set_parameter_vector_float( + "com_gain", parameters.com_gain + ) + self.zmp_controller.initialize(controller_param_handler) + + def define_integrator(self): + self.system = blf.continuous_dynamical_system.FloatingBaseSystemKinematics() + manif_rot = blf.conversions.to_manif_rot(self.H_b[:3, :3]) + self.system.set_state( + ( + self.H_b[:3, 3], + manif_rot, + self.s, + ) + ) + + self.integrator = ( + blf.continuous_dynamical_system.FloatingBaseSystemKinematicsForwardEulerIntegrator() + ) + self.integrator.set_dynamical_system(self.system) + self.integrator.set_integration_step(self.frequency) + + def run(self): + controller_succeded = self.solver.advance() + return controller_succeded + + def pose_to_matrix(self, position, quaternion): + """ + Convert position (xyz) and quaternion (wxyz) to a transformation matrix. + + Parameters: + - position (list or numpy array): 3D position [x, y, z] + - quaternion (list or numpy array): Quaternion [w, x, y, z] + + Returns: + - transformation_matrix (numpy array): 4x4 transformation matrix + """ + + # Create a 3x3 rotation matrix from the quaternion + rotation_matrix = Rotation.from_quat(quaternion.coeffs()).as_matrix() + + # Create a 4x4 transformation matrix + transformation_matrix = np.eye(4) + transformation_matrix[:3, :3] = rotation_matrix + transformation_matrix[:3, 3] = position + + return transformation_matrix + + def get_output(self): + # GET IK RESULTS + base_velocity = self.solver.get_output().base_velocity + joint_vel = self.solver.get_output().joint_velocity + self.system.set_control_input((base_velocity.coeffs(), joint_vel)) + self.integrator.integrate( + datetime.timedelta(seconds=0), datetime.timedelta(seconds=self.frequency) + ) + base_position, base_rotation, joint_position = self.integrator.get_solution() + return joint_position + + def update_state(self): + base_velocity = self.solver.get_output().base_velocity + joint_vel = self.solver.get_output().joint_velocity + base_position, base_rotation, joint_position = self.integrator.get_solution() + H_b = self.pose_to_matrix(base_position, base_rotation) + self.kindyn.setRobotState( + H_b, joint_position, base_velocity.coeffs(), joint_vel, self.gravity + ) + + def update_task_references_mpc( + self, + com, + dcom, + ddcom, + left_foot_desired, + right_foot_desired, + s_desired, + wrenches_left, + wrenches_right, + H_omega, + ): + self.solver.get_task("LEFT_FOOT").set_set_point( + left_foot_desired.transform, + manifpy.SE3Tangent(left_foot_desired.mixed_velocity), + ) + self.solver.get_task("RIGHT_FOOT").set_set_point( + right_foot_desired.transform, + manifpy.SE3Tangent(right_foot_desired.mixed_velocity), + ) + self.solver.get_task("JOINT_REGULARIZATION").set_set_point( + s_desired, np.zeros(self.robot_model.NDoF) + ) + zmp_des = self.compute_zmp_wrench(wrenches_left, wrenches_right) + com_vel_control = np.zeros(3) + # # define the input + controller_input = blf.simplified_model_controllers.CoMZMPControllerInput() + controller_input.desired_CoM_position = com[:2] + controller_input.desired_CoM_velocity = dcom[:2] + controller_input.desired_ZMP_position = zmp_des + controller_input.CoM_position = self.com[:2] + controller_input.ZMP_position = self.zmp + controller_input.angle = 0 + + self.zmp_controller.set_input(controller_input) + self.zmp_controller.advance() + com_vel_control[:2] = self.zmp_controller.get_output() + com_vel_control[2] = dcom[2] + com_new = self.com[:, 0] + com_vel_control * self.frequency + com_vel_control = dcom + self.solver.get_task("COM").set_set_point(com, com_vel_control) + + def set_state_with_base(self, s, s_dot, H_b, w_b, t): + self.s = s + self.s_dot = s_dot + self.t = t + self.w_b = w_b + self.H_b = H_b + self.kindyn.setRobotState(self.H_b, self.s, self.w_b, self.s_dot, self.gravity) + + def set_desired_base_orientation(self): + manif_rot = blf.conversions.to_manif_rot(self.H_b[:3, :3]) + self.solver.get_task("CHEST").set_set_point( + manif_rot, manifpy.SO3Tangent.Zero() + ) + self.solver.get_task("ROOT_TASK").set_set_point(self.H_b[:3, 3], np.zeros(3)) + + def set_state(self, s, s_dot, t): + self.s = s + self.s_dot = s_dot + self.t = t + contact_frames_pose = { + self.robot_model.left_foot_frame: np.eye(4), + self.robot_model.right_foot_frame: np.eye(4), + } + contact_frames_list = [ + self.robot_model.left_foot_frame, + self.robot_model.right_foot_frame, + ] + self.H_b = self.robot_model.get_base_pose_from_contacts( + self.s, contact_frames_pose + ) + self.w_b = iDynTree.Twist() + self.w_b = self.robot_model.get_base_velocity_from_contacts( + self.H_b, self.s, self.s_dot, contact_frames_list + ) + self.kindyn.setRobotState(self.H_b, self.s, self.w_b, self.s_dot, self.gravity) + + def update_com(self, H_b, s): + self.com = np.array(self.com_fun(H_b, s)) + + ##Def abstract methods + def get_fitness_parameters(self): + print("to be implemented") + pass + + def compute_zmp( + self, + left_wrench: np.array, + right_wrench: np.array, + ) -> np.array: + """Auxiliary function to retrieve the zero-moment point from the feet wrenches.""" + + # Compute local zmps (one per foot) from the foot wrenches + LF_r_zmp_L = [-left_wrench[4] / left_wrench[2], left_wrench[3] / left_wrench[2]] + RF_r_zmp_R = [ + -right_wrench[4] / right_wrench[2], + right_wrench[3] / right_wrench[2], + ] + + # Express the local zmps in homogeneous coordinates + LF_r_zmp_L_homogenous = np.array([LF_r_zmp_L[0], LF_r_zmp_L[1], 0, 1]) + RF_r_zmp_R_homogenous = np.array([RF_r_zmp_R[0], RF_r_zmp_R[1], 0, 1]) + + # Retrieve the global transform of the feet frames + W_H_LF = ( + self.kindyn.getWorldTransform(self.robot_model.left_foot_frame) + .asHomogeneousTransform() + .toNumPy() + ) + W_H_RF = ( + self.kindyn.getWorldTransform(self.robot_model.right_foot_frame) + .asHomogeneousTransform() + .toNumPy() + ) + + # Express the local zmps (one per foot) in a common reference frame (i.e. the world frame) + W_r_zmp_L_hom = W_H_LF @ LF_r_zmp_L_homogenous + W_r_zmp_L = W_r_zmp_L_hom[0:2] + W_r_zmp_R_hom = W_H_RF @ RF_r_zmp_R_homogenous + W_r_zmp_R = W_r_zmp_R_hom[0:2] + + # Compute the global zmp as a weighted mean of the local zmps (one per foot) + # expressed in a common reference frame (i.e. the world frame) + if np.linalg.norm(left_wrench) < 1e-6: + # if the left foot is not in contact, the zmp is the rigth + W_r_zmp_global = W_r_zmp_R + elif np.linalg.norm(right_wrench) < 1e-6: + W_r_zmp_global = W_r_zmp_L + else: + W_r_zmp_global = W_r_zmp_L * ( + left_wrench[2] / (left_wrench[2] + right_wrench[2]) + ) + W_r_zmp_R * (right_wrench[2] / (left_wrench[2] + right_wrench[2])) + + self.zmp = W_r_zmp_global + return W_r_zmp_global + + def compute_zmp_wrench(self, left_force, right_force) -> np.array: + left_wrench = np.zeros(6) + left_wrench[:3] = left_force + right_wrench = np.zeros(6) + right_wrench[:3] = right_force + """Auxiliary function to retrieve the zero-moment point from the feet wrenches.""" + + # Compute local zmps (one per foot) from the foot wrenches + LF_r_zmp_L = [-left_wrench[4] / left_wrench[2], left_wrench[3] / left_wrench[2]] + RF_r_zmp_R = [ + -right_wrench[4] / right_wrench[2], + right_wrench[3] / right_wrench[2], + ] + + # Express the local zmps in homogeneous coordinates + LF_r_zmp_L_homogenous = np.array([LF_r_zmp_L[0], LF_r_zmp_L[1], 0, 1]) + RF_r_zmp_R_homogenous = np.array([RF_r_zmp_R[0], RF_r_zmp_R[1], 0, 1]) + + # Retrieve the global transform of the feet frames + W_H_LF = ( + self.kindyn.getWorldTransform(self.robot_model.left_foot_frame) + .asHomogeneousTransform() + .toNumPy() + ) + W_H_RF = ( + self.kindyn.getWorldTransform(self.robot_model.right_foot_frame) + .asHomogeneousTransform() + .toNumPy() + ) + + # Express the local zmps (one per foot) in a common reference frame (i.e. the world frame) + W_r_zmp_L_hom = W_H_LF @ LF_r_zmp_L_homogenous + W_r_zmp_L = W_r_zmp_L_hom[0:2] + W_r_zmp_R_hom = W_H_RF @ RF_r_zmp_R_homogenous + W_r_zmp_R = W_r_zmp_R_hom[0:2] + + # Compute the global zmp as a weighted mean of the local zmps (one per foot) + # expressed in a common reference frame (i.e. the world frame) + if np.linalg.norm(left_wrench) < 1e-6: + # if the left foot is not in contact, the zmp is the rigth + W_r_zmp_global = W_r_zmp_R + elif np.linalg.norm(right_wrench) < 1e-6: + W_r_zmp_global = W_r_zmp_L + else: + W_r_zmp_global = W_r_zmp_L * ( + left_wrench[2] / (left_wrench[2] + right_wrench[2]) + ) + W_r_zmp_R * (right_wrench[2] / (left_wrench[2] + right_wrench[2])) + + return W_r_zmp_global diff --git a/src/comodo/inverseKinematics/inverseKinematicsParamTuning.py b/src/comodo/inverseKinematics/inverseKinematicsParamTuning.py new file mode 100644 index 0000000..062edff --- /dev/null +++ b/src/comodo/inverseKinematics/inverseKinematicsParamTuning.py @@ -0,0 +1,66 @@ +import numpy as np + + +class InverseKinematicsParamTuning: + def __init__(self) -> None: + self.zmp_gain = [0.4, 0.4] + self.com_gain = [4.3, 4.3] + self.foot_linear = 1.0 + self.foot_angular = 6.59 + self.com_linear = 2.59 + self.chest_angular = 1.4 + self.root_linear = 2.8 + self.kp_joint_tracking = 5.0 + self.weigth_joint = [ + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 2.0, + 2.0, + 2.0, + 2.0, + 2.0, + 2.0, + 2.0, + 2.0, + ] + + def set_parameters( + self, + zmp_gain, + com_gain, + foot_linear, + foot_angular, + com_linear, + chest_angular, + root_linear, + ): + self.zmp_gain = [zmp_gain, zmp_gain] + self.com_gain = [com_gain, com_gain] + self.foot_linear = foot_linear + self.foot_angular = foot_angular + self.com_linear = com_linear + self.chest_angular = chest_angular + self.root_linear = root_linear + + def set_from_xk(self, x_k): + # x_k[zmp_gain_1, zmp_gain_2, com_gain_1,com_gain_2, foot_linear, foot_angular, com_linear, chest_angular, root_linear] + # dimension 9 + self.set_parameters( + zmp_gain=x_k[0], + com_gain=x_k[1], + foot_linear=x_k[2], + foot_angular=x_k[3], + com_linear=x_k[4], + chest_angular=x_k[5], + root_linear=x_k[6], + ) diff --git a/src/comodo/mujocoSimulator/mujocoSimulator.py b/src/comodo/mujocoSimulator/mujocoSimulator.py index cf95c3b..8814108 100644 --- a/src/comodo/mujocoSimulator/mujocoSimulator.py +++ b/src/comodo/mujocoSimulator/mujocoSimulator.py @@ -3,19 +3,25 @@ import math import numpy as np import mujoco_viewer +import copy +import casadi as cs class MujocoSimulator(Simulator): def __init__(self) -> None: + self.desired_pos = None + self.postion_control = False + self.compute_misalignment_gravity_fun() super().__init__() def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None): self.robot_model = robot_model + mujoco_xml = robot_model.get_mujoco_model() self.model = mujoco.MjModel.from_xml_string(mujoco_xml) - # mujoco.mj_saveLastXML("/home/carlotta/iit_ws/element_hardware-intelligence/muj_mod.xml",self.model) - # mujoco.MjModel.mj_saveModel() self.data = mujoco.MjData(self.model) + self.create_mapping_vector_from_mujoco() + self.create_mapping_vector_to_mujoco() mujoco.mj_forward(self.model, self.data) self.set_joint_vector_in_mujoco(s) self.set_base_pose_in_mujoco(xyz_rpy=xyz_rpy) @@ -26,6 +32,18 @@ def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None): self.kv_motors = ( kv_motors if not (kv_motors is None) else np.zeros(self.robot_model.NDoF) ) + self.H_left_foot = copy.deepcopy(self.robot_model.H_left_foot) + self.H_right_foot = copy.deepcopy(self.robot_model.H_right_foot) + self.H_left_foot_num = None + self.H_right_foot_num = None + self.mass = self.robot_model.get_total_mass() + + + def get_contact_status(self): + left_wrench, rigth_wrench = self.get_feet_wrench() + left_foot_contact = left_wrench[2] > 0.1*self.mass + right_foot_contact = rigth_wrench[2] > 0.1*self.mass + return left_foot_contact, right_foot_contact def set_visualize_robot_flag(self, visualize_robot): self.visualize_robot_flag = visualize_robot @@ -40,35 +58,84 @@ def set_base_pose_in_mujoco(self, xyz_rpy): self.data.qpos[:7] = base_xyz_quat def set_joint_vector_in_mujoco(self, pos): + pos_muj = self.convert_vector_to_mujoco(pos) indexes_joint = self.model.jnt_qposadr[1:] for i in range(self.robot_model.NDoF): - self.data.qpos[indexes_joint[i]] = pos[i] + self.data.qpos[indexes_joint[i]] = pos_muj[i] def set_input(self, input): - self.data.ctrl = input - np.copyto(self.data.ctrl, input) + input_muj = self.convert_vector_to_mujoco(input) + self.data.ctrl = input_muj + np.copyto(self.data.ctrl, input_muj) + + def set_position_input(self, pos): + pos_muj = self.convert_vector_to_mujoco(pos) + self.desired_pos = pos_muj + self.postion_control = True + + def create_mapping_vector_to_mujoco(self): + # This function creates the to_mujoco map + self.to_mujoco = [] + for mujoco_joint in self.robot_model.mujoco_joint_order: + try: + index = self.robot_model.joint_name_list.index(mujoco_joint) + self.to_mujoco.append(index) + except ValueError: + raise ValueError(f"Mujoco joint '{mujoco_joint}' not found in joint list.") + + def create_mapping_vector_from_mujoco(self): + # This function creates the to_mujoco map + self.from_mujoco = [] + for joint in self.robot_model.joint_name_list: + try: + index = self.robot_model.mujoco_joint_order.index(joint) + self.from_mujoco.append(index) + except ValueError: + raise ValueError(f"Joint name list joint '{joint}' not found in mujoco list.") + + def convert_vector_to_mujoco (self, array_in): + out_muj = np.asarray([array_in[self.to_mujoco[item]] for item in range(self.robot_model.NDoF)]) + return out_muj + + def convert_from_mujoco(self, array_muj): + out_classic = np.asarray([array_muj[self.from_mujoco[item]] for item in range(self.robot_model.NDoF)]) + return out_classic def step(self, n_step=1, visualize=True): - mujoco.mj_step(self.model, self.data, n_step) - mujoco.mj_step1(self.model, self.data) - mujoco.mj_forward(self.model, self.data) - if self.visualize_robot_flag and visualize: + if self.postion_control: + for _ in range(n_step): + s, s_dot, tau = self.get_state(use_mujoco_convention=True) + kp_muj = self.convert_vector_to_mujoco(self.robot_model.kp_position_control) + kd_muj = self.convert_vector_to_mujoco(self.robot_model.kd_position_control) + ctrl = ( + kp_muj * (self.desired_pos - s) + - kd_muj * s_dot + ) + self.data.ctrl = ctrl + np.copyto(self.data.ctrl, ctrl) + mujoco.mj_step(self.model, self.data) + mujoco.mj_step1(self.model, self.data) + mujoco.mj_forward(self.model, self.data) + else: + mujoco.mj_step(self.model, self.data, n_step) + mujoco.mj_step1(self.model, self.data) + mujoco.mj_forward(self.model, self.data) + + if self.visualize_robot_flag: self.viewer.render() def step_with_motors(self, n_step, torque): - indexes_joint_acceleration = self.model.jnt_dofadr[1:] s_dot_dot = self.data.qacc[indexes_joint_acceleration[0] :] for _ in range(n_step): - indexes_joint_acceleration = self.model.jnt_dofadr[1:] s_dot_dot = self.data.qacc[indexes_joint_acceleration[0] :] s_dot = self.data.qvel[indexes_joint_acceleration[0] :] input = np.asarray( [ - self.Im[item] * s_dot_dot[item] - + self.kv_motors[item] * s_dot[item] - + torque[item] + self.Im[self.to_mujoco[item]] * s_dot_dot[item] + + self.kv_motors[self.to_mujoco[item]] * s_dot[item] + + torque[self.to_mujoco[item]] for item in range(self.robot_model.NDoF) ] ) @@ -78,9 +145,78 @@ def step_with_motors(self, n_step, torque): if self.visualize_robot_flag: self.viewer.render() + def compute_misalignment_gravity_fun(self): + H = cs.SX.sym("H", 4, 4) + theta = cs.SX.sym("theta") + theta = cs.dot([0, 0, 1], H[:3, 2]) - 1 + error = cs.Function("error", [H], [theta]) + self.error_mis = error + + def check_feet_status(self, s, H_b): + left_foot_pose = self.robot_model.H_left_foot(H_b, s) + rigth_foot_pose = self.robot_model.H_right_foot(H_b, s) + left_foot_z = left_foot_pose[2, 3] + rigth_foot_z = rigth_foot_pose[2, 3] + left_foot_contact = not (left_foot_z > 0.1) + rigth_foot_contact = not (rigth_foot_z > 0.1) + misalignment_left = self.error_mis(left_foot_pose) + misalignment_rigth = self.error_mis(rigth_foot_pose) + left_foot_condition = abs(left_foot_contact * misalignment_left) + rigth_foot_condition = abs(rigth_foot_contact * misalignment_rigth) + misalignment_error = left_foot_condition + rigth_foot_condition + if ( + abs(left_foot_contact * misalignment_left) > 0.02 + or abs(rigth_foot_contact * misalignment_rigth) > 0.02 + ): + return False, misalignment_error + + return True, misalignment_error + + def get_feet_wrench(self): + left_foot_wrench = np.zeros(6) + rigth_foot_wrench = np.zeros(6) + s,s_dot, tau = self.get_state() + H_b = self.get_base() + self.H_left_foot_num = np.array(self.H_left_foot(H_b, s)) + self.H_right_foot_num = np.array(self.H_right_foot(H_b, s)) + for i in range(self.data.ncon): + contact = self.data.contact[i] + c_array = np.zeros(6, dtype=np.float64) + mujoco.mj_contactForce(self.model, self.data, i, c_array) + name_contact = mujoco.mj_id2name( + self.model, mujoco.mjtObj.mjOBJ_GEOM, int(contact.geom[1]) + ) + w_H_contact = np.eye(4) + w_H_contact[:3, :3] = contact.frame.reshape(3, 3) + w_H_contact[:3, 3] = contact.pos + if ( + name_contact == self.robot_model.right_foot_rear_ct + or name_contact == self.robot_model.right_foot_front_ct + ): + RF_H_contact = np.linalg.inv(self.H_right_foot_num) @ w_H_contact + wrench_RF = self.compute_resulting_wrench(RF_H_contact, c_array) + rigth_foot_wrench[:] += wrench_RF.reshape(6) + elif ( + name_contact == self.robot_model.left_foot_front_ct + or name_contact == self.robot_model.left_foot_rear_ct + ): + LF_H_contact = np.linalg.inv(self.H_left_foot_num) @ w_H_contact + wrench_LF = self.compute_resulting_wrench(LF_H_contact, c_array) + left_foot_wrench[:] += wrench_LF.reshape(6) + return (left_foot_wrench, rigth_foot_wrench) + + def compute_resulting_wrench(self, b_H_a, force_torque_a): + p = b_H_a[:3, 3] + R = b_H_a[:3, :3] + adjoint_matrix = np.zeros([6, 6]) + adjoint_matrix[:3, :3] = R + adjoint_matrix[3:, :3] = np.cross(p, R) + adjoint_matrix[3:, 3:] = R + force_torque_b = adjoint_matrix @ force_torque_a.reshape(6, 1) + return force_torque_b + # note that for mujoco the ordering is w,x,y,z def get_base(self): - indexes_joint = self.model.jnt_qposadr[1:] # Extract quaternion components w, x, y, z = self.data.qpos[3 : indexes_joint[0]] @@ -121,13 +257,18 @@ def get_base_velocity(self): indexes_joint_velocities = self.model.jnt_dofadr[1:] return self.data.qvel[: indexes_joint_velocities[0]] - def get_state(self): + def get_state(self, use_mujoco_convention = False): indexes_joint = self.model.jnt_qposadr[1:] indexes_joint_velocities = self.model.jnt_dofadr[1:] s = self.data.qpos[indexes_joint[0] :] s_dot = self.data.qvel[indexes_joint_velocities[0] :] tau = self.data.ctrl - return s, s_dot, tau + if(use_mujoco_convention): + return s,s_dot,tau + s_out = self.convert_from_mujoco(s) + s_dot_out = self.convert_from_mujoco(s_dot) + tau_out = self.convert_from_mujoco(tau) + return s_out, s_dot_out, tau_out def close(self): if self.visualize_robot_flag: diff --git a/src/comodo/robotModel/robotModel.py b/src/comodo/robotModel/robotModel.py index 98fd8d8..d4a59d2 100644 --- a/src/comodo/robotModel/robotModel.py +++ b/src/comodo/robotModel/robotModel.py @@ -9,7 +9,8 @@ import idyntree.bindings as iDynTree import casadi as cs import copy -import xml.etree.ElementTree as ET +import resolve_robotics_uri_py as rru +from pathlib import Path class RobotModel(KinDynComputations): @@ -22,7 +23,17 @@ def __init__( left_foot: str = "l_sole", right_foot: str = "r_sole", torso: str = "chest", + right_foot_rear_link_name: str = "r_foot_rear", + right_foot_front_link_name: str = "r_foot_front", + left_foot_rear_link_name: str = "l_foot_rear", + left_foot_front_link_name: str = "l_foot_front", + kp_pos_control: np.float32 = np.array( + [35 * 70.0, 35 * 70.0, 35 * 40.0, 35 * 100.0, 35 * 100.0, 35 * 100.0,35 * 70.0, 35 * 70.0, 35 * 40.0, 35 * 100.0, 35 * 100.0, 35 * 100.0,20 * 5.745, 20 * 5.745, 20 * 5.745, 20 * 1.745,20 * 5.745, 20 * 5.745, 20 * 5.745, 20 * 1.745] + ), + kd_pos_control: np.float32= np.array([15 * 0.15, 15 * 0.15, 15 * 0.35, 15 * 0.15, 15 * 0.15, 15 * 0.15,15 * 0.15, 15 * 0.15, 15 * 0.35, 15 * 0.15, 15 * 0.15, 15 * 0.15,4 * 5.745, 4 * 5.745, 4 * 5.745, 4 * 1.745,4 * 5.745, 4 * 5.745, 4 * 5.745, 4 * 1.745]) ) -> None: + self.collision_keyword = "_collision" + self.visual_keyword = "_visual" self.urdf_string = urdfstring self.robot_name = robot_name self.joint_name_list = joint_name_list @@ -30,6 +41,10 @@ def __init__( self.left_foot_frame = left_foot self.right_foot_frame = right_foot self.torso_link = torso + self.right_foot_rear_ct = right_foot_rear_link_name + self.collision_keyword + self.right_foot_front_ct = right_foot_front_link_name + self.collision_keyword + self.left_foot_rear_ct = left_foot_rear_link_name + self.collision_keyword + self.left_foot_front_ct = left_foot_front_link_name + self.collision_keyword self.remote_control_board_list = [ "/" + self.robot_name + "/torso", @@ -39,7 +54,9 @@ def __init__( "/" + self.robot_name + "/right_leg", ] - # self.mujoco_lines_urdf = ' ' + self.kp_position_control = kp_pos_control + self.kd_position_control = kd_pos_control + self.ki_position_control = 10 * self.kd_position_control self.gravity = iDynTree.Vector3() self.gravity.zero() self.gravity.setVal(2, -9.81) @@ -47,6 +64,8 @@ def __init__( path_temp_xml = tempfile.NamedTemporaryFile(mode="w+") path_temp_xml.write(urdfstring) super().__init__(path_temp_xml.name, self.joint_name_list, self.base_link) + self.H_left_foot = self.forward_kinematics_fun(self.left_foot_frame) + self.H_right_foot = self.forward_kinematics_fun(self.right_foot_frame) def override_control_boar_list(self, remote_control_board_list: list): self.remote_control_board_list = remote_control_board_list @@ -85,8 +104,6 @@ def compute_desired_position_walking(self): self.solver = cs.Opti() self.solver.solver("ipopt", p_opts, s_opts) - self.H_left_foot = self.forward_kinematics_fun(self.left_foot_frame) - self.H_right_foot = self.forward_kinematics_fun(self.right_foot_frame) self.w_H_torso = self.forward_kinematics_fun(self.torso_link) self.s = self.solver.variable(self.NDoF) # joint positions self.quat_pose_b = self.solver.variable(7) @@ -104,11 +121,6 @@ def compute_desired_position_walking(self): H_torso = self.w_H_torso(H_b, self.s) quat_torso = self.rotation_matrix_to_quaternion(H_torso[:3, :3]) reference_rotation = np.asarray([1.0, 0.0, 0.0, 0.0]) - - # self.solver.subject_to(cs.abs(self.s[11])> 0.05 ) - # self.solver.subject_to(self.s[11]< -0.05) - # self.solver.subject_to(cs.abs(self.s[17])>0.05 ) - # self.solver.subject_to(self.s[17]< -0.05) self.solver.subject_to(self.s[17] == desired_knee) self.solver.subject_to(self.s[11] == desired_knee) self.solver.subject_to(self.s[3] == elbow) @@ -116,7 +128,6 @@ def compute_desired_position_walking(self): self.solver.subject_to(self.s[1] == shoulder_roll) self.solver.subject_to(self.s[5] == shoulder_roll) self.solver.subject_to(self.s[9] == self.s[15]) - # self.solver.subject_to(cs.norm_2(self.quat_pose_b[:4]) == 1.0) self.solver.subject_to(H_left_foot[2, 3] == 0.0) self.solver.subject_to(H_right_foot[2, 3] == 0.0) self.solver.subject_to(quat_left_foot == reference_rotation) @@ -125,8 +136,6 @@ def compute_desired_position_walking(self): cost_function = cs.sumsqr(self.s) cost_function += cs.sumsqr(H_left_foot[:2, 3] - left_foot_pos[:2]) cost_function += cs.sumsqr(H_right_foot[:2, 3] - right_foot_pos[:2]) - # cost_function += cs.sumsqr(self.s[11] - desired_knee) - # cost_function += cs.sumsqr(self.s[17] - desired_knee) self.solver.minimize(cost_function) self.sol = self.solver.solve() s_return = np.array(self.sol.value(self.s)) @@ -179,7 +188,7 @@ def rotation_matrix_to_quaternion(self, R): z = (R[1, 0] - R[0, 1]) / S # Normalize the quaternion - length = cs.sqrt(w ** 2 + x ** 2 + y ** 2 + z ** 2) + length = cs.sqrt(w**2 + x**2 + y**2 + z**2) w /= length x /= length y /= length @@ -188,43 +197,78 @@ def rotation_matrix_to_quaternion(self, R): return cs.vertcat(w, x, y, z) def get_mujoco_urdf_string(self): + ## We first start by ET tempFileOut = tempfile.NamedTemporaryFile(mode="w+") tempFileOut.write(copy.deepcopy(self.urdf_string)) - robot = URDF.load(tempFileOut.name) - - ## Adding floating joint to have floating base system - for item in robot.joints: - if item.name not in (self.joint_name_list): - item.joint_type = "fixed" - world_joint = Joint("floating_joint", "floating", "world", self.base_link) - world_link = Link("world", None, None, None) - robot._links.append(world_link) - robot._joints.append(world_joint) - temp_urdf = tempfile.NamedTemporaryFile(mode="w+") - robot.save(temp_urdf.name) - tree = ET.parse(temp_urdf.name) - root = tree.getroot() + root = ET.fromstring(self.urdf_string) + self.mujoco_joint_order = [] + # Declaring as fixed the not controlled joints + for joint in root.findall(".//joint"): + joint_name = joint.attrib.get("name") + if joint_name not in self.joint_name_list: + joint.set("type", "fixed") + else: + self.mujoco_joint_order.append(joint_name) + + new_link = ET.Element("link") + new_link.set("name", "world") + root.append(new_link) + floating_joint = ET.Element("joint") + floating_joint.set("name", "floating_joint") + floating_joint.set("type", "floating") + # Create parent element + parent = ET.Element("parent") + parent.set("link", "world") + floating_joint.append(parent) + + # Create child element + child = ET.Element("child") + child.set("link", self.base_link) + floating_joint.append(child) + + # Append the new joint element to the root + root.append(floating_joint) + + ## Adding the name to the collision and link visual + for link in root.findall(".//link"): + link_name = link.attrib.get("name") + # Check if a collision element with a name exists + collision_elements = link.findall("./collision") + if not any( + collision.find("name") is not None for collision in collision_elements + ): + # If no collision element with a name exists, add one + if collision_elements: + # If there are collision elements, append name to the first one + collision_elements[0].set( + "name", link_name + self.collision_keyword + ) + visual_elements = link.findall("./visual") + if not any(visual.find("name") is not None for visual in visual_elements): + # If no collision element with a name exists, add one + if visual_elements: + visual_elements[0].set("name", link_name + self.visual_keyword) + meshes = self.get_mesh_path(root) robot_el = None for elem in root.iter(): if elem.tag == "robot": robot_el = elem break - ## Adding compiler discard visaul false for mujoco rendering mujoco_el = ET.Element("mujoco") compiler_el = ET.Element("compiler") compiler_el.set("discardvisual", "false") + if not (meshes is None): + compiler_el.set("meshdir", str(meshes)) mujoco_el.append(compiler_el) robot_el.append(mujoco_el) # Convert the XML tree to a string robot_urdf_string_original = ET.tostring(root) - # urdf_string_temp = temp_urdf.read() return robot_urdf_string_original def get_mujoco_model(self): urdf_string = self.get_mujoco_urdf_string() - # urdf_string = urdf_mujoco_file.read() - mujoco_model = mujoco.MjModel.from_xml_string(urdf_string) + mujoco_model = mujoco.MjModel.from_xml_string(urdf_string) path_temp_xml = tempfile.NamedTemporaryFile(mode="w+") mujoco.mj_saveLastXML(path_temp_xml.name, mujoco_model) @@ -239,7 +283,7 @@ def get_mujoco_model(self): break actuator_entry = ET.Element("actuator") - for name_joint in self.joint_name_list: + for name_joint in self.mujoco_joint_order: new_motor_entry = ET.Element("motor") new_motor_entry.set("name", name_joint) new_motor_entry.set("joint", name_joint) @@ -445,3 +489,22 @@ def get_centroidal_momentum(self): Jcm = self.get_centroidal_momentum_jacobian() nu = np.concatenate((self.w_b.toNumPy(), self.ds.toNumPy())) return Jcm @ nu + + def get_mesh_path(self, robot_urdf: ET.Element) -> Path: + """ + Get the mesh path from the robot urdf. + + Args: + robot_urdf (ET.Element): The robot urdf. + + Returns: + Path: The mesh path. + """ + # find the mesh path + mesh = robot_urdf.find(".//mesh") + if mesh is None: + return None + path = mesh.attrib["filename"] + mesh_path = rru.resolve_robotics_uri(path).parent + + return mesh_path