141 lines
3.0 KiB
Plaintext
141 lines
3.0 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"from Go2Py.sim.mujoco import Go2Sim\n",
|
|
"from Go2Py.robot.model import Go2Model\n",
|
|
"import pinocchio as pin \n",
|
|
"import numpy as np\n",
|
|
"import time"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"model = Go2Model()\n",
|
|
"model.get"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"robot = Go2Sim()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"robot.step()"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"omega = robot.getIMU()['gyro']\n",
|
|
"def hat(w):\n",
|
|
" return np.array([[0., -w[2], w[1]],\n",
|
|
" [w[2], 0., -w[0]],\n",
|
|
" [-w[1], w[0], 0.]])"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"robot.standUpReset()\n",
|
|
"while True:\n",
|
|
" time.sleep(0.001)\n",
|
|
" robot.setCommands(robot.standing_q, np.zeros(12), np.ones(12)*100, np.ones(12)*0.1, np.zeros(12))\n",
|
|
" robot.step() \n",
|
|
" trans, quat = robot.getPose()\n",
|
|
" state = robot.getJointStates()\n",
|
|
" quat = pin.Quaternion(np.hstack([quat[1:],quat[0]]))\n",
|
|
" R = quat.matrix()\n",
|
|
" T = np.eye(4)\n",
|
|
" T[0:3,0:3] = R\n",
|
|
" vel = np.zeros(6)\n",
|
|
" model.update(state['q'], state['dq'],T,vel)\n",
|
|
"\n",
|
|
" J = model.getInfo()['J']['FR_foot'][:,6:]\n",
|
|
" nle = model.getInfo()['nle'][6:]\n",
|
|
" # tau = (robot.data.qfrc_smooth+robot.data.qfrc_constraint)[6:]\n",
|
|
" tau = state['tau_est'].squeeze()\n",
|
|
" print(np.linalg.pinv(J.T)[0:3]@(tau - nle))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"question: What exactly is the difference between the local world aligned and local jacobians in the pinocchio? Why it fits my expectations for the FR3 and not the Go2?"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"robot.getJointStates()\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"J[3:,:].T"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": []
|
|
}
|
|
],
|
|
"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
|
|
}
|