EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HysteresisContactDetector added

This commit is contained in:
Rooholla-KhorramBakht 2024-04-05 15:59:21 -04:00
parent b788f8ad2a
commit b6a24a93d1
7 changed files with 308 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
Go2Py/estimation/__init__.py Normal file
View File

14
Go2Py/estimation/contact.py Normal file
View File

@ -0,0 +1,14 @@
import numpy as np
class HysteresisContactDetector:
def __init__(self, upper_limit, lower_limit):
self.upper_limit = upper_limit
self.lower_limit = lower_limit
self.contact_state = np.zeros(4)
def update(self, contact_forces):
self.contact_state[np.where(contact_forces>self.upper_limit)[0]]=1
self.contact_state[np.where(contact_forces<self.lower_limit)[0]]=0
def getContactStates(self):
return self.contact_state

11
Go2Py/robot/model.py
View File

@ -156,8 +156,8 @@ class Go2Model:
self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q_)[self.dq_reordering_idx,:]
self.Minv_ = self.Minv_[:,self.dq_reordering_idx]
for ef_frame in self.ef_frames:
# J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL)
J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
# J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL)
self.ef_J_[ef_frame]=J[:, self.dq_reordering_idx]
def getInfo(self):
@ -174,3 +174,10 @@ class Go2Model:
'g':self.g_,
'J':self.ef_J_,
}
def getGroundReactionForce(self, tau_est, body_acceleration=None):
if body_acceleration is None:
grf = {key:np.linalg.pinv(self.ef_J_[key][:3,6:].T)@(tau_est.squeeze() - self.nle_[6:]) for key in self.ef_J_.keys()}
else:
raise NotImplementedError("Ground reaction force with body dynamics is not implemented")
return grf

192
examples/contact_detector.ipynb
View File

File diff suppressed because one or more lines are too long

12
examples/fsm_sim.ipynb
View File

@ -26,17 +26,9 @@
},
{
"cell_type": "code",
"execution_count": 6,
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"controller timeout\n"
]
}
],
"outputs": [],
"source": [
"robot.sitDownReset()\n",
"fsm = FSM(robot, remote, safety_hypervisor)"

32
examples/lowlevel_dds_interface.ipynb
View File

@ -2,9 +2,18 @@
"cells": [
{
"cell_type": "code",
"execution_count": null,
"execution_count": 1,
"metadata": {},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
]
}
],
"source": [
"from Go2Py.robot.interface.dds import GO2Real\n",
"import time\n",
@ -13,17 +22,26 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"q = robot.getJointStates()['q']"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"import time\n",
"for i in range(1000):\n",
" q = np.zeros(12) \n",
"for i in range(1000000):\n",
" # q = np.zeros(12) \n",
" dq = np.zeros(12)\n",
" kp = np.zeros(12)\n",
" kd = np.zeros(12)\n",
" kp = np.ones(12)*20\n",
" kd = np.ones(12)*0.3\n",
" tau = np.zeros(12)\n",
" tau[0] = -0.0\n",
" robot.setCommands(q, dq, kp, kd, tau)\n",

115
examples/walk_these_ways_sim.ipynb
View File

@ -9,7 +9,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
@ -19,7 +19,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
@ -29,7 +29,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
@ -45,9 +45,17 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 4,
"metadata": {},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
}
],
"source": [
"agent = WalkTheseWaysAgent(cfg, command_profile, robot)\n",
"agent = HistoryWrapper(agent)"
@ -55,7 +63,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 5,
"metadata": {},
"outputs": [],
"source": [
@ -66,9 +74,20 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 6,
"metadata": {},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"frq: 15.967047985229458 Hz\n",
"frq: 42.47353444522081 Hz\n",
"frq: 43.69977078558033 Hz\n",
"frq: 40.20806211954177 Hz\n"
]
}
],
"source": [
"robot.reset()\n",
"obs = agent.reset()\n",
@ -93,7 +112,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
@ -107,7 +126,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
@ -116,7 +135,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
@ -149,7 +168,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
@ -160,9 +179,31 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 5,
"metadata": {},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"p_gains: [20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20. 20.]\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"frq: 0.10237921921805228 Hz\n",
"frq: 46.14348108298404 Hz\n",
"frq: 44.397794032030994 Hz\n",
"frq: 45.603148715941465 Hz\n",
"frq: 45.86595514341641 Hz\n",
"frq: 44.16591026356524 Hz\n",
"frq: 45.82636627843454 Hz\n",
"frq: 45.51950772169346 Hz\n"
]
}
],
"source": [
"checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
"# checkpoint = \"/home/rstaion/projects/rooholla/walk-these-ways/runs/gait-conditioned-agility/pretrain-v0/train/025417.456545\"\n",
@ -172,18 +213,56 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 7,
"metadata": {},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"frq: 45.741406386320016 Hz\n",
"frq: 44.6901430961184 Hz\n",
"frq: 44.451434445775085 Hz\n",
"frq: 45.19042385846963 Hz\n",
"frq: 45.1087737411542 Hz\n",
"frq: 46.690013692073066 Hz\n",
"frq: 45.09228519824546 Hz\n",
"frq: 45.70053825535531 Hz\n",
"frq: 46.01388873651992 Hz\n",
"frq: 44.94924554183813 Hz\n",
"frq: 45.49383372200228 Hz\n",
"frq: 44.522684330085134 Hz\n",
"frq: 44.42130458266699 Hz\n",
"frq: 43.71525649844704 Hz\n",
"frq: 44.37383889464886 Hz\n"
]
}
],
"source": [
"controller.command_profile.pitch_cmd=0.0\n",
"controller.command_profile.body_height_cmd=0.1\n",
"controller.command_profile.body_height_cmd=0.0\n",
"controller.command_profile.footswing_height_cmd=0.08\n",
"controller.command_profile.roll_cmd=0.0\n",
"controller.command_profile.stance_width_cmd=0.2\n",
"controller.command_profile.x_vel_cmd=-0.2\n",
"controller.command_profile.y_vel_cmd=0.01\n",
"controller.command_profile.setGaitType(\"pronking\")"
"controller.command_profile.setGaitType(\"trotting\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"controller.command_profile.pitch_cmd=0.0\n",
"controller.command_profile.body_height_cmd=0.0\n",
"controller.command_profile.footswing_height_cmd=0.08\n",
"controller.command_profile.roll_cmd=0.0\n",
"controller.command_profile.stance_width_cmd=0.2\n",
"controller.command_profile.x_vel_cmd=-0.2\n",
"controller.command_profile.y_vel_cmd=0.01\n",
"controller.command_profile.setGaitType(\"trotting\")"
]
},
{