161 lines
3.8 KiB
Plaintext
161 lines
3.8 KiB
Plaintext
{
|
|
"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
|
|
}
|