diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface/dds.py
index a78758e..72ab0d1 100644
--- a/Go2Py/robot/interface/dds.py
+++ b/Go2Py/robot/interface/dds.py
@@ -16,6 +16,8 @@ from cyclonedds.sub import DataReader
from cyclonedds.util import duration
from Go2Py.unitree_go.msg.dds_ import LowState_
from threading import Thread
+from scipy.spatial.transform import Rotation
+
class GO2Real():
def __init__(
@@ -190,4 +192,10 @@ class GO2Real():
return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)
def overheat(self):
- return False
\ No newline at end of file
+ return False
+
+ def getGravityInBody(self):
+ q = self.getIMU()['quat']
+ R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
+ g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
+ return g_in_body
\ No newline at end of file
diff --git a/Go2Py/robot/interface/ros2.py b/Go2Py/robot/interface/ros2.py
index 205d360..297f627 100644
--- a/Go2Py/robot/interface/ros2.py
+++ b/Go2Py/robot/interface/ros2.py
@@ -14,6 +14,7 @@ from Go2Py.joy import xKeySwitch, xRockerBtn
from geometry_msgs.msg import TwistStamped
from Go2Py.msgs.unitree_go.msg import LowState, Go2pyLowCmd
from nav_msgs.msg import Odometry
+from scipy.spatial.transform import Rotation
@@ -253,4 +254,10 @@ class GO2Real(Node):
return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)
def overheat(self):
- return False
\ No newline at end of file
+ return False
+
+ def getGravityInBody(self):
+ q = self.getIMU()['quat']
+ R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
+ g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
+ return g_in_body
\ No newline at end of file
diff --git a/examples/lowlevel_dds_interface.ipynb b/examples/lowlevel_dds_interface.ipynb
index 2ecb065..8eeef53 100644
--- a/examples/lowlevel_dds_interface.ipynb
+++ b/examples/lowlevel_dds_interface.ipynb
@@ -46,17 +46,21 @@
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 2,
"metadata": {},
"outputs": [],
- "source": []
+ "source": [
+ "from scipy.spatial.transform import Rotation"
+ ]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": []
+ "source": [
+ "Rotation.from_matrix(np.eye(3)).as_quat()"
+ ]
}
],
"metadata": {
@@ -75,7 +79,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
- "version": "3.8.10"
+ "version": "3.8.18"
}
},
"nbformat": 4,
diff --git a/examples/walk_these_ways_sim.ipynb b/examples/walk_these_ways_sim.ipynb
index 25d3211..2f1461b 100644
--- a/examples/walk_these_ways_sim.ipynb
+++ b/examples/walk_these_ways_sim.ipynb
@@ -84,8 +84,8 @@
" if i % (control_dt // simulation_dt) == 0:\n",
" obs, ret, done, info = agent.step(action)\n",
" robot.step()\n",
- " command_profile.yaw_vel_cmd = 1.2\n",
- " command_profile.x_vel_cmd = 0.8\n",
+ " command_profile.yaw_vel_cmd = 0.0\n",
+ " command_profile.x_vel_cmd = 0.0\n",
" command_profile.y_vel_cmd = 0.0\n",
" command_profile.stance_width_cmd=0.2\n",
" command_profile.footswing_height_cmd=-0.05\n",
@@ -102,7 +102,7 @@
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
@@ -116,7 +116,7 @@
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
@@ -134,21 +134,22 @@
" def init(self):\n",
" self.obs = self.agent.reset()\n",
" self.policy_info = {}\n",
- "\n",
- " def update(self, robot, remote):\n",
- " action = self.policy(self.obs, self.policy_info)\n",
- " self.obs, self.ret, self.done, self.info = self.agent.step(action)\n",
- " self.command_profile.yaw_vel_cmd = 1.2\n",
- " self.command_profile.x_vel_cmd = 0.8\n",
+ " self.command_profile.yaw_vel_cmd = 0.0\n",
+ " self.command_profile.x_vel_cmd = 0.0\n",
" self.command_profile.y_vel_cmd = 0.0\n",
" self.command_profile.stance_width_cmd=0.2\n",
" self.command_profile.footswing_height_cmd=-0.05\n",
- " self.command_profile.step_frequency_cmd = 2.5\n"
+ " self.command_profile.step_frequency_cmd = 2.5\n",
+ " self.command_profile.bodyHeight = 0.05\n",
+ "\n",
+ " def update(self, robot, remote):\n",
+ " action = self.policy(self.obs, self.policy_info)\n",
+ " self.obs, self.ret, self.done, self.info = self.agent.step(action)\n"
]
},
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
@@ -160,14 +161,48 @@
},
{
"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": [
"checkpoint = \"/home/rstaion/projects/rooholla/locomotion/Go2Py/Go2Py/assets/checkpoints/walk_these_ways/\"\n",
"controller = walkTheseWaysController(robot, remote, checkpoint)\n",
"fsm = FSM(robot, remote, safety_hypervisor, user_controller_callback=controller.update)"
]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [
+ {
+ "ename": "",
+ "evalue": "",
+ "output_type": "error",
+ "traceback": [
+ "\u001b[1;31mThe Kernel crashed while executing code in the the current cell or a previous cell. Please review the code in the cell(s) to identify a possible cause of the failure. Click here for more info. View Jupyter log for further details."
+ ]
+ }
+ ],
+ "source": [
+ "robot.standUpReset()\n",
+ "controller.command_profile.roll_cmd=0.3"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": []
}
],
"metadata": {