From 37429f661dc854f30d4b54aa6833b6ef6f8bd185 Mon Sep 17 00:00:00 2001 From: Rooholla-Khorrambakht Date: Thu, 28 Mar 2024 13:32:00 -0400 Subject: [PATCH] actuatornet notebook added --- examples/actuator_net.ipynb | 160 +++++++++++++++++ pin_model.ipynb | 336 +++++++++++++++++++++++++++--------- 2 files changed, 417 insertions(+), 79 deletions(-) create mode 100644 examples/actuator_net.ipynb diff --git a/examples/actuator_net.ipynb b/examples/actuator_net.ipynb new file mode 100644 index 0000000..a196cec --- /dev/null +++ b/examples/actuator_net.ipynb @@ -0,0 +1,160 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.robot.fsm import FSM\n", + "from Go2Py.robot.model import Go2Model\n", + "from Go2Py.sim.mujoco import Go2Sim\n", + "import numpy as np" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "robot = Go2Sim()" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[0.3868 0. 0. ]\n", + "0.3868\n", + "0.093\n", + "0.09550000000000002\n", + "0.213\n", + "0.213\n" + ] + } + ], + "source": [ + "model = Go2Model()" + ] + }, + { + "cell_type": "code", + "execution_count": 26, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "The task space position of the robot feet are:\n", + "{'base_link': array([0. , 0. , 0.34]), 'FR_foot': array([ 0.19400515, -0.142 , 0.0372495 ]), 'FL_foot': array([0.19811581, 0.142 , 0.03813916]), 'RR_foot': array([-0.19303592, -0.142 , 0.03357947]), 'RL_foot': array([-0.1897387, 0.142 , 0.0330523])}\n" + ] + } + ], + "source": [ + "T0 = np.eye(4) \n", + "T0[2,-1]=0.34\n", + "q0 = robot.standing_q\n", + "x0 = model.forwardKinematics(T0, q0)\n", + "x0 = {key:x[0:3,-1] for key,x in zip(x0.keys(),x0.values())}\n", + "print('The task space position of the robot feet are:')\n", + "print(x0)\n", + "x0 = np.hstack([x0[key] for key in ['FR_foot', 'FL_foot', 'RR_foot', 'RL_foot']])\n" + ] + }, + { + "cell_type": "code", + "execution_count": 28, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "array([ 0.00000000e+00, -2.22044605e-16, 2.22044605e-16, 0.00000000e+00,\n", + " 2.22044605e-16, -4.44089210e-16, 0.00000000e+00, 1.11022302e-16,\n", + " 0.00000000e+00, 0.00000000e+00, -1.11022302e-16, 0.00000000e+00])" + ] + }, + "execution_count": 28, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "model.inverseKinematics(T0,x0)-q0" + ] + }, + { + "cell_type": "code", + "execution_count": 52, + "metadata": {}, + "outputs": [], + "source": [ + "dT = 1/50.\n", + "amplitudes = [0.01*(i+1) for i in range(8)]\n", + "frequencies = [0.5*(i+1) for i in range(40)]\n", + "durations = []\n", + "\n", + "for f in frequencies:\n", + " durations.append(len(amplitudes)*[int(f+1)*(2*np.pi/f)])\n", + "durations = np.array(durations).reshape(-1).tolist()" + ] + }, + { + "cell_type": "code", + "execution_count": 55, + "metadata": {}, + "outputs": [], + "source": [ + "x = []\n", + "for i,a in enumerate(amplitudes):\n", + " for j,f in enumerate(frequencies):\n", + " for N in range(int(durations[i*j]//dT)):\n", + " t = N/durations[i*j]\n", + " x_s = a*np.cos(2*np.pi*f*t)\n", + " y_s = a*np.sin(2*np.pi*f*t)\n", + " x.append(x_s)\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import matplotlib.pyplot as = [] # List of Pose\n", + "\n", + "\n", + "(plan, fraction) = move_group.compute_cartesian_path, eef_step = 0.01, jump_threshold = 0.0)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "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.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/pin_model.ipynb b/pin_model.ipynb index ada4aa1..5bf284e 100644 --- a/pin_model.ipynb +++ b/pin_model.ipynb @@ -131,7 +131,7 @@ }, { "cell_type": "code", - "execution_count": 21, + "execution_count": 3, "metadata": {}, "outputs": [], "source": [ @@ -164,7 +164,7 @@ " self.M_ = self.robot.mass(q_)[[self.dq_reordering_idx],[self.dq_reordering_idx]]\n", " self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q_)[[self.dq_reordering_idx],[self.dq_reordering_idx]]\n", " for ef_frame in self.ef_frames:\n", - " J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL)\n", + " J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)\n", " self.ef_J_[ef_frame]=J[:, self.dq_reordering_idx]\n", "\n", " def getInfo(self):\n", @@ -179,7 +179,7 @@ }, { "cell_type": "code", - "execution_count": 22, + "execution_count": 4, "metadata": {}, "outputs": [], "source": [ @@ -188,7 +188,7 @@ }, { "cell_type": "code", - "execution_count": 23, + "execution_count": 5, "metadata": {}, "outputs": [], "source": [ @@ -197,93 +197,271 @@ }, { "cell_type": "code", - "execution_count": 25, + "execution_count": 8, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.sim.mujoco import Go2Sim\n", + "robot = Go2Sim()" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [], + "source": [ + "robot.step()\n", + "mujoco_params = robot.getDynamicsParams()" + ] + }, + { + "cell_type": "code", + "execution_count": 67, + "metadata": {}, + "outputs": [], + "source": [ + "states = robot.getJointStates()\n", + "q = states['q']\n", + "dq = states['dq']\n", + "trans, quat = robot.getPose()\n", + "quat = np.hstack([quat[1:], quat[0]])\n", + "q = np.hstack([trans, quat, q])" + ] + }, + { + "cell_type": "code", + "execution_count": 102, + "metadata": {}, + "outputs": [], + "source": [ + "dq_reordering_idx = np.array([0, 1, 2, 3, 4, 5,\\\n", + " 9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14])" + ] + }, + { + "cell_type": "code", + "execution_count": 125, "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "{'FR_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , 0.142 , 0. ,\n", - " -0.426 , -0.213 , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 1. , 0. , 0.426 , 0. , 0.1934, 0.426 ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 1. , -0.142 , -0.1934, 0. , -0.0955,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 1. , 0. , 0. , 1. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n", - " 1. , 1. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ]]),\n", - " 'FL_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , -0.142 , 0. ,\n", - " 0. , 0. , 0. , -0.426 , -0.213 , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 1. , 0. , 0.426 , 0. , 0.1934, 0. ,\n", - " 0. , 0. , 0.426 , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 1. , 0.142 , -0.1934, 0. , 0. ,\n", - " 0. , 0. , 0.0955, 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n", - " 0. , 0. , 1. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n", - " 0. , 0. , 0. , 1. , 1. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ]]),\n", - " 'RR_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , 0.142 , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , -0.426 ,\n", - " -0.213 , 0. , 0. , 0. ],\n", - " [ 0. , 1. , 0. , 0.426 , 0. , -0.1934, 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0.426 , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 1. , -0.142 , 0.1934, 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , -0.0955, 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", - " 0. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 1. ,\n", - " 1. , 0. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ]]),\n", - " 'RL_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , -0.142 , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , -0.426 , -0.213 ],\n", - " [ 0. , 1. , 0. , 0.426 , 0. , -0.1934, 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0.426 , 0. , 0. ],\n", - " [ 0. , 0. , 1. , 0.142 , 0.1934, 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0.0955, 0. , 0. ],\n", - " [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 1. , 0. , 0. ],\n", - " [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 1. , 1. ],\n", - " [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n", - " 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n", - " 0. , 0. , 0. , 0. ]])}" + "-0.5194361885075903" ] }, - "execution_count": 25, + "execution_count": 125, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "model.getInfo()['J']" + "M1 = model.robot.mass(q)[dq_reordering_idx,:]\n", + "M1=M1[:,dq_reordering_idx]\n", + "# M2 = robot.getDynamicsParams()['M']\n", + "(M1-M2).sum()\n", + "# M1.shape" + ] + }, + { + "cell_type": "code", + "execution_count": 126, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "" + ] + }, + "execution_count": 126, + "metadata": {}, + "output_type": "execute_result" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "import matplotlib.pyplot as plt\n", + "plt.imshow(M1[6:,6:])" + ] + }, + { + "cell_type": "code", + "execution_count": 117, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "" + ] + }, + "execution_count": 117, + "metadata": {}, + "output_type": "execute_result" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "plt.imshow(M2[6:,6:])" + ] + }, + { + "cell_type": "code", + "execution_count": 129, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "15.206408000000003" + ] + }, + "execution_count": 129, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "robot.model.body_mass.sum()" + ] + }, + { + "cell_type": "code", + "execution_count": 135, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "15.019000000000004" + ] + }, + "execution_count": 135, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "s = model.robot.model\n", + "sum([i.mass for i in s.inertias])" + ] + }, + { + "cell_type": "code", + "execution_count": 97, + "metadata": {}, + "outputs": [], + "source": [ + "Jp, Jr = robot.getSiteJacobian('FL_foot')\n", + "model.robot.computeJointJacobians(q).T\n", + "J = model.robot.getFrameJacobian(model.robot.model.getFrameId('FL_foot'), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)" + ] + }, + { + "cell_type": "code", + "execution_count": 143, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "(0.009002122197148882, 0.13167880433018087)" + ] + }, + "execution_count": 143, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "(Jp.T-J[0:3,:].T).sum(),(Jr.T-J[3:,:].T).sum()" + ] + }, + { + "cell_type": "code", + "execution_count": 136, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "" + ] + }, + "execution_count": 136, + "metadata": {}, + "output_type": "execute_result" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "plt.imshow(Jp)" + ] + }, + { + "cell_type": "code", + "execution_count": 138, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "" + ] + }, + "execution_count": 138, + "metadata": {}, + "output_type": "execute_result" + }, + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": { + "needs_background": "light" + }, + "output_type": "display_data" + } + ], + "source": [ + "plt.imshow(J[0:3,dq_reordering_idx])" ] }, {