FSM added
This commit is contained in:
Rooholla-KhorramBakht
parent
e76acc01a3
commit
592dfb3eb8
|
|
@ -36,10 +36,17 @@ class Go2Sim:
|
||||||
self.render = render
|
self.render = render
|
||||||
self.step_counter = 0
|
self.step_counter = 0
|
||||||
|
|
||||||
self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153,
|
self.sitting_q = np.array([ 0.0, 1.26186061, -2.81310153,
|
||||||
0.03325212, 1.25883281, -2.78329301,
|
0.0, 1.25883281, -2.78329301,
|
||||||
-0.34708387, 1.27193761, -2.8052032 ,
|
0.0, 1.27193761, -2.8052032 ,
|
||||||
0.32040933, 1.27148342, -2.81436563])
|
0.0, 1.27148342, -2.81436563])
|
||||||
|
|
||||||
|
self.standing_q = np.array([ 0.0, 0.77832842, -1.56065452,
|
||||||
|
0.0, 0.76754963, -1.56634164,
|
||||||
|
0.0, 0.76681757, -1.53601146,
|
||||||
|
0.0, 0.75422204, -1.53229916])
|
||||||
|
|
||||||
|
self.q0 = self.sitting_q
|
||||||
self.pos0 = np.array([0., 0., 0.1])
|
self.pos0 = np.array([0., 0., 0.1])
|
||||||
self.rot0 = np.array([1., 0., 0., 0.])
|
self.rot0 = np.array([1., 0., 0., 0.])
|
||||||
self.reset()
|
self.reset()
|
||||||
|
|
@ -50,6 +57,13 @@ class Go2Sim:
|
||||||
self.jacr = np.zeros((3, self.nv))
|
self.jacr = np.zeros((3, self.nv))
|
||||||
self.M = np.zeros((self.nv, self.nv))
|
self.M = np.zeros((self.nv, self.nv))
|
||||||
|
|
||||||
|
self.q_des = np.zeros(12)
|
||||||
|
self.dq_des = np.zeros(12)
|
||||||
|
self.tau_ff = np.zeros(12)
|
||||||
|
self.kp = np.zeros(12)
|
||||||
|
self.kv = np.zeros(12)
|
||||||
|
self.latest_command_stamp = time.time()
|
||||||
|
|
||||||
def reset(self):
|
def reset(self):
|
||||||
self.q_nominal = np.hstack(
|
self.q_nominal = np.hstack(
|
||||||
[self.pos0.squeeze(), self.rot0.squeeze(), self.q0.squeeze()]
|
[self.pos0.squeeze(), self.rot0.squeeze(), self.q0.squeeze()]
|
||||||
|
|
@ -57,22 +71,16 @@ class Go2Sim:
|
||||||
self.data.qpos = self.q_nominal
|
self.data.qpos = self.q_nominal
|
||||||
self.data.qvel = np.zeros(18)
|
self.data.qvel = np.zeros(18)
|
||||||
|
|
||||||
def standUp(self):
|
def standUpReset(self):
|
||||||
self.q0 = np.array([ 0.00901526, 0.77832842, -1.56065452,
|
self.q0 = self.standing_q
|
||||||
-0.00795561, 0.76754963, -1.56634164,
|
|
||||||
-0.05375515, 0.76681757, -1.53601146,
|
|
||||||
0.06183922, 0.75422204, -1.53229916])
|
|
||||||
self.pos0 = np.array([0., 0., 0.33])
|
self.pos0 = np.array([0., 0., 0.33])
|
||||||
self.rot0 = np.array([1., 0., 0., 0.])
|
self.rot0 = np.array([1., 0., 0., 0.])
|
||||||
self.reset()
|
self.reset()
|
||||||
mujoco.mj_step(self.model, self.data)
|
mujoco.mj_step(self.model, self.data)
|
||||||
self.viewer.sync()
|
self.viewer.sync()
|
||||||
|
|
||||||
def sitDown(self):
|
def sitDownReset(self):
|
||||||
self.q0 = np.array([-0.03479636, 1.26186061, -2.81310153,
|
self.q0 = self.sitting_q
|
||||||
0.03325212, 1.25883281, -2.78329301,
|
|
||||||
-0.34708387, 1.27193761, -2.8052032 ,
|
|
||||||
0.32040933, 1.27148342, -2.81436563])
|
|
||||||
self.pos0 = np.array([0., 0., 0.1])
|
self.pos0 = np.array([0., 0., 0.1])
|
||||||
self.rot0 = np.array([1., 0., 0., 0.])
|
self.rot0 = np.array([1., 0., 0., 0.])
|
||||||
self.reset()
|
self.reset()
|
||||||
|
|
@ -95,12 +103,19 @@ class Go2Sim:
|
||||||
return self.data.sensordata[6:10]
|
return self.data.sensordata[6:10]
|
||||||
|
|
||||||
def setCommands(self, q_des, dq_des, kp, kv, tau_ff):
|
def setCommands(self, q_des, dq_des, kp, kv, tau_ff):
|
||||||
q, dq = self.getJointStates()
|
self.q_des = q_des
|
||||||
tau = np.diag(kp)@(q_des-q).reshape(12,1)+ \
|
self.dq_des = dq_des
|
||||||
np.diag(kv)@(dq_des-dq).reshape(12,1)+tau_ff.reshape(12,1)
|
self.kp = kp
|
||||||
self.data.ctrl[:] = tau.squeeze()
|
self.kv = kv
|
||||||
|
self.tau_ff = tau_ff
|
||||||
|
self.latest_command_stamp = time.time()
|
||||||
|
|
||||||
def step(self):
|
def step(self):
|
||||||
|
q, dq = self.getJointStates()
|
||||||
|
tau = np.diag(self.kp)@(self.q_des-q).reshape(12,1)+ \
|
||||||
|
np.diag(self.kv)@(self.dq_des-dq).reshape(12,1)+self.tau_ff.reshape(12,1)
|
||||||
|
self.data.ctrl[:] = tau.squeeze()
|
||||||
|
|
||||||
self.step_counter += 1
|
self.step_counter += 1
|
||||||
mujoco.mj_step(self.model, self.data)
|
mujoco.mj_step(self.model, self.data)
|
||||||
# Render every render_ds_ratio steps (60Hz GUI update)
|
# Render every render_ds_ratio steps (60Hz GUI update)
|
||||||
|
|
@ -121,5 +136,8 @@ class Go2Sim:
|
||||||
'nle':nle
|
'nle':nle
|
||||||
}
|
}
|
||||||
|
|
||||||
|
def overheat(self):
|
||||||
|
return False
|
||||||
|
|
||||||
def close(self):
|
def close(self):
|
||||||
self.viewer.close()
|
self.viewer.close()
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,420 @@
|
||||||
|
{
|
||||||
|
"cells": [
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 1,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"from Go2Py.sim.mujoco import Go2Sim\n",
|
||||||
|
"import numpy as np"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 2,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"robot = Go2Sim()\n",
|
||||||
|
"robot.sitDownReset()"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 3,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"import threading\n",
|
||||||
|
"from pynput import keyboard\n",
|
||||||
|
"class BaseRemote:\n",
|
||||||
|
" def __init__(self):\n",
|
||||||
|
" pass\n",
|
||||||
|
" def startSeq(self):\n",
|
||||||
|
" return False\n",
|
||||||
|
" def standUpDownSeq(self):\n",
|
||||||
|
" return False\n",
|
||||||
|
"\n",
|
||||||
|
"remote = BaseRemote()\n",
|
||||||
|
"\n",
|
||||||
|
"class KeyboardRemote(BaseRemote):\n",
|
||||||
|
" def __init__(self):\n",
|
||||||
|
" super().__init__()\n",
|
||||||
|
" self.start_seq_flag = False\n",
|
||||||
|
" self.stand_up_down_seq_flag = False\n",
|
||||||
|
" self.listener_thread = threading.Thread(target=self._listen_to_keyboard)\n",
|
||||||
|
" self.listener_thread.daemon = True\n",
|
||||||
|
" self.listener_thread.start()\n",
|
||||||
|
"\n",
|
||||||
|
" def _on_press(self, key):\n",
|
||||||
|
" try:\n",
|
||||||
|
" if key.char == 's': # Start sequence\n",
|
||||||
|
" self.start_seq_flag = True\n",
|
||||||
|
" elif key.char == 'u': # Stand up/down sequence\n",
|
||||||
|
" self.stand_up_down_seq_flag = True\n",
|
||||||
|
" except AttributeError:\n",
|
||||||
|
" pass # Special keys (like space) will be handled here\n",
|
||||||
|
"\n",
|
||||||
|
" def _listen_to_keyboard(self):\n",
|
||||||
|
" with keyboard.Listener(on_press=self._on_press) as listener:\n",
|
||||||
|
" listener.join()\n",
|
||||||
|
"\n",
|
||||||
|
" def startSeq(self):\n",
|
||||||
|
" if self.start_seq_flag:\n",
|
||||||
|
" self.start_seq_flag = False\n",
|
||||||
|
" return True\n",
|
||||||
|
" return False\n",
|
||||||
|
"\n",
|
||||||
|
" def standUpDownSeq(self):\n",
|
||||||
|
" if self.stand_up_down_seq_flag:\n",
|
||||||
|
" self.stand_up_down_seq_flag = False\n",
|
||||||
|
" return True\n",
|
||||||
|
" return False"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 4,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"remote = KeyboardRemote()"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 5,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n",
|
||||||
|
"False\n"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"ename": "KeyboardInterrupt",
|
||||||
|
"evalue": "",
|
||||||
|
"output_type": "error",
|
||||||
|
"traceback": [
|
||||||
|
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
|
||||||
|
"\u001b[0;31mKeyboardInterrupt\u001b[0m Traceback (most recent call last)",
|
||||||
|
"Cell \u001b[0;32mIn[5], line 4\u001b[0m\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mwhile\u001b[39;00m \u001b[38;5;28;01mTrue\u001b[39;00m:\n\u001b[1;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(remote\u001b[38;5;241m.\u001b[39mstandUpDownSeq())\n\u001b[0;32m----> 4\u001b[0m \u001b[43mtime\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43msleep\u001b[49m\u001b[43m(\u001b[49m\u001b[38;5;241;43m0.1\u001b[39;49m\u001b[43m)\u001b[49m\n",
|
||||||
|
"\u001b[0;31mKeyboardInterrupt\u001b[0m: "
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"import time\n",
|
||||||
|
"while True:\n",
|
||||||
|
" print(remote.standUpDownSeq())\n",
|
||||||
|
" time.sleep(0.1)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 5,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"class SafetyHypervisor():\n",
|
||||||
|
" def __init__(self, robot):\n",
|
||||||
|
" self.robot = robot\n",
|
||||||
|
"\n",
|
||||||
|
" def unsafe(self):\n",
|
||||||
|
" return True\n",
|
||||||
|
" \n",
|
||||||
|
" def controlTimeout(self):\n",
|
||||||
|
" if time.time() - robot.latest_command_stamp > 0.1:\n",
|
||||||
|
" print('controller timeout')\n",
|
||||||
|
" return True\n",
|
||||||
|
" else:\n",
|
||||||
|
" return False \n",
|
||||||
|
"\n",
|
||||||
|
"safety = SafetyHypervisor(robot)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 9,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"import threading\n",
|
||||||
|
"import numpy as np\n",
|
||||||
|
"import time\n",
|
||||||
|
"from enum import Enum\n",
|
||||||
|
"\n",
|
||||||
|
"\n",
|
||||||
|
"class FSM:\n",
|
||||||
|
" def __init__(self, robot, remote, safety_hypervisor):\n",
|
||||||
|
" self.robot = robot\n",
|
||||||
|
" self.remote = remote\n",
|
||||||
|
" self.safety = safety_hypervisor\n",
|
||||||
|
"\n",
|
||||||
|
" self.states = [ \"pre_standing\", \\\n",
|
||||||
|
" \"standing\", \\\n",
|
||||||
|
" \"locked_stance\", \\\n",
|
||||||
|
" \"user_loop\" ,\\\n",
|
||||||
|
" \"sitting\", \\\n",
|
||||||
|
" \"damping\"]\n",
|
||||||
|
"\n",
|
||||||
|
" self.state = \"damping\"\n",
|
||||||
|
"\n",
|
||||||
|
" self.tracking_kp = np.array(4*[200, 200, 350.]).reshape(12)\n",
|
||||||
|
" self.tracking_kv = np.array(12*[10.])\n",
|
||||||
|
" self.tracking_complete = True\n",
|
||||||
|
" self.damping_kv = np.array(12*[3.])\n",
|
||||||
|
" self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), self.damping_kv, np.zeros(12))\n",
|
||||||
|
" self.dT = 1./50.\n",
|
||||||
|
"\n",
|
||||||
|
" self.modes = {\"tracking\":self.trackingControlUpdate,\n",
|
||||||
|
" \"damping\" :self.dampingControlUpdate,\n",
|
||||||
|
" \"user\": self.userControlUpdate,\n",
|
||||||
|
" }\n",
|
||||||
|
" self.setMode(\"damping\")\n",
|
||||||
|
"\n",
|
||||||
|
" self.running = True\n",
|
||||||
|
" self.fsm_thread = threading.Thread(target = self.update)\n",
|
||||||
|
" self.fsm_thread.start()\n",
|
||||||
|
"\n",
|
||||||
|
" self.sim_thread = threading.Thread(target=self.simUpdate)\n",
|
||||||
|
" self.sim_thread.start()\n",
|
||||||
|
"\n",
|
||||||
|
" def setMode(self, mode):\n",
|
||||||
|
" assert mode in self.modes.keys(), 'the requested control update mode is not implemented'\n",
|
||||||
|
" self.updateCommands = self.modes[mode]\n",
|
||||||
|
" \n",
|
||||||
|
" def moveTo(self, target, duration=0.5):\n",
|
||||||
|
" # assert self.tracking_complete, 'The previous moveTo command is not completed yet!'\n",
|
||||||
|
" self.q_start = self.robot.getJointStates()[0]\n",
|
||||||
|
" self.q_target = target\n",
|
||||||
|
" self.time = 0.0\n",
|
||||||
|
" self.move_duration = duration\n",
|
||||||
|
" self.q_des = lambda t: [self.q_start + np.clip((t)/self.move_duration,0, 1)*(self.q_target - self.q_start), \\\n",
|
||||||
|
" True if np.clip((t)/self.move_duration,0, 1)==1 else False] # q_des(t), Movement finished\n",
|
||||||
|
" self.tracking_complete = False\n",
|
||||||
|
" self.setMode(\"tracking\")\n",
|
||||||
|
"\n",
|
||||||
|
" def trackingControlUpdate(self):\n",
|
||||||
|
" self.time +=self.dT\n",
|
||||||
|
" q_des, done = self.q_des(self.time)\n",
|
||||||
|
" self.robot.setCommands(q_des, np.zeros(12), self.tracking_kp, self.tracking_kv, np.zeros(12))\n",
|
||||||
|
" self.tracking_complete = done\n",
|
||||||
|
"\n",
|
||||||
|
" def dampingControlUpdate(self):\n",
|
||||||
|
" self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), self.damping_kv, np.zeros(12))\n",
|
||||||
|
"\n",
|
||||||
|
" def userControlUpdate(self):\n",
|
||||||
|
" pass\n",
|
||||||
|
"\n",
|
||||||
|
" def simUpdate(self):\n",
|
||||||
|
" while self.running:\n",
|
||||||
|
" self.robot.step()\n",
|
||||||
|
" time.sleep(robot.dt)\n",
|
||||||
|
" \n",
|
||||||
|
" def update(self):\n",
|
||||||
|
" while self.running:\n",
|
||||||
|
" getattr(self, self.state)()\n",
|
||||||
|
" time.sleep(self.dT)\n",
|
||||||
|
" self.updateCommands()\n",
|
||||||
|
" \n",
|
||||||
|
" def damping(self):\n",
|
||||||
|
" # print('damping')\n",
|
||||||
|
" if self.remote.standUpDownSeq():\n",
|
||||||
|
" self.moveTo(robot.sitting_q, 1)\n",
|
||||||
|
" self.state = \"pre_standing\"\n",
|
||||||
|
" else:\n",
|
||||||
|
" self.setMode(\"damping\")\n",
|
||||||
|
" self.state = \"damping\"\n",
|
||||||
|
"\n",
|
||||||
|
" def pre_standing(self):\n",
|
||||||
|
" # print(\"pre_stance\")\n",
|
||||||
|
" if self.tracking_complete:\n",
|
||||||
|
" self.moveTo(robot.standing_q, duration=2)\n",
|
||||||
|
" self.state = 'standing'\n",
|
||||||
|
" else:\n",
|
||||||
|
" self.state = \"pre_standing\"\n",
|
||||||
|
" \n",
|
||||||
|
" def standing(self):\n",
|
||||||
|
" # print(\"standing\")\n",
|
||||||
|
" if self.tracking_complete:\n",
|
||||||
|
" self.moveTo(robot.standing_q, duration=2)\n",
|
||||||
|
" self.state = 'locked_stance'\n",
|
||||||
|
" else:\n",
|
||||||
|
" self.state = \"standing\"\n",
|
||||||
|
"\n",
|
||||||
|
" def locked_stance(self):\n",
|
||||||
|
" # print(\"locked_stance\")\n",
|
||||||
|
" if self.remote.startSeq():\n",
|
||||||
|
" self.setMode(\"user\")\n",
|
||||||
|
" self.state = \"user_loop\"\n",
|
||||||
|
" self.robot.setCommands(np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12), np.zeros(12))\n",
|
||||||
|
" elif self.remote.standUpDownSeq() or robot.overheat():\n",
|
||||||
|
" self.moveTo(robot.sitting_q, duration = 3)\n",
|
||||||
|
" self.state = \"sitting\"\n",
|
||||||
|
"\n",
|
||||||
|
" def user_loop(self):\n",
|
||||||
|
" print(\"user_loop\")\n",
|
||||||
|
" if self.safety.unsafe():\n",
|
||||||
|
" self.setMode(\"damping\")\n",
|
||||||
|
" elif self.remote.standUpDownSeq() or self.safety.controlTimeout():\n",
|
||||||
|
" self.moveTo(robot.standing_q, duration = 1)\n",
|
||||||
|
" self.state = \"locked_stance\"\n",
|
||||||
|
" else:\n",
|
||||||
|
" self.state = \"user_loop\"\n",
|
||||||
|
"\n",
|
||||||
|
" def sitting(self):\n",
|
||||||
|
" # print('sitting')\n",
|
||||||
|
" if self.tracking_complete:\n",
|
||||||
|
" self.setMode(\"damping\")\n",
|
||||||
|
" self.state = 'damping'\n",
|
||||||
|
" else:\n",
|
||||||
|
" self.state = \"sitting\"\n",
|
||||||
|
" \n",
|
||||||
|
" def close(self):\n",
|
||||||
|
" self.running = False"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 10,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n",
|
||||||
|
"user_loop\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"fsm = FSM(robot, remote, safety)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": null,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"fsm.moveTo(robot.sitting_q, 1)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": null,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"fsm.moveTo(robot.standing_q, 2)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": null,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"fsm.setMode(\"damping\")"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 8,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"fsm.close()\n",
|
||||||
|
"robot.sitDownReset()\n"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": null,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": []
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"metadata": {
|
||||||
|
"kernelspec": {
|
||||||
|
"display_name": "b1-env",
|
||||||
|
"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.18"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"nbformat": 4,
|
||||||
|
"nbformat_minor": 2
|
||||||
|
}
|
||||||
|
|
@ -2,7 +2,7 @@
|
||||||
"cells": [
|
"cells": [
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 1,
|
"execution_count": null,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [],
|
"outputs": [],
|
||||||
"source": [
|
"source": [
|
||||||
|
|
@ -12,7 +12,7 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 2,
|
"execution_count": null,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [],
|
"outputs": [],
|
||||||
"source": [
|
"source": [
|
||||||
|
|
@ -76,49 +76,9 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 4,
|
"execution_count": null,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [
|
"outputs": [],
|
||||||
{
|
|
||||||
"data": {
|
|
||||||
"text/plain": [
|
|
||||||
"(array([[ 1.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
|
|
||||||
" 0.00000000e+00, -3.03599143e-01, 1.39266782e-01,\n",
|
|
||||||
" 0.00000000e+00, -3.02750500e-01, -1.51075728e-01,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
|
|
||||||
" [ 0.00000000e+00, 1.00000000e+00, 0.00000000e+00,\n",
|
|
||||||
" 3.03599143e-01, 0.00000000e+00, 1.94005151e-01,\n",
|
|
||||||
" 3.03599143e-01, 5.45551624e-06, 1.35362601e-03,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
|
|
||||||
" [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00,\n",
|
|
||||||
" -1.39266782e-01, -1.94005151e-01, 0.00000000e+00,\n",
|
|
||||||
" -9.27667816e-02, -6.05126024e-04, -1.50144238e-01,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
|
|
||||||
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n",
|
|
||||||
" array([[0. , 0. , 0. , 1. , 0. ,\n",
|
|
||||||
" 0. , 1. , 0. , 0. , 0. ,\n",
|
|
||||||
" 0. , 0. , 0. , 0. , 0. ,\n",
|
|
||||||
" 0. , 0. , 0. ],\n",
|
|
||||||
" [0. , 0. , 0. , 0. , 1. ,\n",
|
|
||||||
" 0. , 0. , 0.99995936, 0.99995936, 0. ,\n",
|
|
||||||
" 0. , 0. , 0. , 0. , 0. ,\n",
|
|
||||||
" 0. , 0. , 0. ],\n",
|
|
||||||
" [0. , 0. , 0. , 0. , 0. ,\n",
|
|
||||||
" 1. , 0. , 0.00901514, 0.00901514, 0. ,\n",
|
|
||||||
" 0. , 0. , 0. , 0. , 0. ,\n",
|
|
||||||
" 0. , 0. , 0. ]]))"
|
|
||||||
]
|
|
||||||
},
|
|
||||||
"execution_count": 4,
|
|
||||||
"metadata": {},
|
|
||||||
"output_type": "execute_result"
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"source": [
|
"source": [
|
||||||
"robot.getSiteJacobian('FR_foot')"
|
"robot.getSiteJacobian('FR_foot')"
|
||||||
]
|
]
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue