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

Merge branch 'main' of github.com:Rooholla-KhorramBakht/Go2Py into main

This commit is contained in:
Rooholla-KhorramBakht 2024-03-25 23:22:34 -04:00
parent 02ebbf2436 e2249cae82
commit ee96c73b54
9 changed files with 469 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Go2Py/assets/urdf/go2.urdf
View File

@ -7,7 +7,7 @@
Stephen Brawner (brawner@gmail.com)
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot name="go2_description">
<robot name="go2">
<link name="base_link">
<inertial>
<origin rpy="0 0 0" xyz="0.021112 0 -0.005366"/>

BIN
Go2Py/assets/urdf/go2/go2.usd
View File

Binary file not shown.

BIN
Go2Py/assets/usd/.thumbs/256x256/go2.usd.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 41 KiB

BIN
Go2Py/assets/usd/go2.usd
View File

Binary file not shown.

1
Go2Py/robot/model.py Normal file
View File

@ -0,0 +1 @@
import pinocchio as pin

16
Go2Py/sim/isaacsim/go2.py
View File

@ -34,15 +34,15 @@ class UnitreeGo2(Articulation):
"""
self._prim_path = prim_path
prim = define_prim(self._prim_path, "Xform")
prim.GetReferences().AddReference(Go2Py.GO2_USD_PATH)
self.usd_path = usd_path
# breakpoint()
if self.usd_path is None:
prim.GetReferences().AddReference(Go2Py.GO2_USD_PATH)
else:
prim.GetReferences().AddReference(self.usd_path)
breakpoint()
super().__init__(
prim_path=self._prim_path,
prim_path=self._prim_path+'/Go2/base',
name=name,
position=position,
orientation=orientation,
@ -51,10 +51,10 @@ class UnitreeGo2(Articulation):
# contact sensor setup
self.feet_order = ["FL", "RL", "FR", "RR"]
self.feet_path = [
self._prim_path + "/FL_foot",
self._prim_path + "/FR_foot",
self._prim_path + "/RL_foot",
self._prim_path + "/RR_foot",
self._prim_path + "/Go2/base/FL_calf",
self._prim_path + "/Go2/base/FR_calf",
self._prim_path + "/Go2/base/RL_calf",
self._prim_path + "/Go2/base/RR_calf",
]
self.color = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1), (1, 1, 0, 1)]
@ -73,7 +73,7 @@ class UnitreeGo2(Articulation):
self._FILTER_WINDOW_SIZE = 20
self._foot_filters = [deque(), deque(), deque(), deque()]
# imu sensor setup
self.imu_path = self._prim_path + "/imu_link"
self.imu_path = self._prim_path + "/Go2/base/base"
self._imu_sensor = IMUSensor(
prim_path=self.imu_path + "/imu_sensor",
name="imu",

86
Go2Py/sim/isaacsim/isaacsim_node.py
View File

@ -3,13 +3,9 @@
import pickle
import sys
import time
import numpy as np
from omni.isaac.kit import SimulationApp
# sys.path.append("/home/vr-station/projects/lcm/build/python")
simulation_app = SimulationApp({"headless": False}) # we can also run as headless.
# import cv2
@ -30,7 +26,11 @@ from Go2Py.sim.utils import (
cfg = load_config(Go2Py.GO2_ISAACSIM_CFG_PATH)
robots = cfg["robots"]
if cfg["cameras"] is None:
cameras = []
else:
cameras = cfg["cameras"]
env_cfg = cfg["environment"]
PHYSICS_DT = 1 / 200
@ -47,6 +47,8 @@ if assets_root_path is None:
print("Could not find Isaac Sim assets folder")
prim = get_prim_at_path(env_cfg["prim_path"])
print("Adding the environment")
if not prim.IsValid():
prim = define_prim(env_cfg["prim_path"], "Xform")
asset_path = (
@ -56,7 +58,7 @@ if not prim.IsValid():
)
prim.GetReferences().AddReference(asset_path)
breakpoint()
print("Adding the robot")
go2 = world.scene.add(
UnitreeGo2(
prim_path=robots[0]["prim_path"],
@ -67,32 +69,30 @@ go2 = world.scene.add(
)
)
# # Add Lidar
# lidar = world.scene.add(
# RotatingLidarPhysX(
# prim_path="/World/Env/GO2/imu_link/lidar",
# name="lidar",
# translation=[0.16, 0.0, 0.14],
# orientation=[0.0, 0.0, 0.0, 1.0],
# )
# )
# Add Lidar if required
if robots[0]["lidar"]:
lidar = world.scene.add(
RotatingLidarPhysX(
prim_path="/World/Env/GO2/Go2/base/base/lidar",
name="lidar",
translation=[0.16, 0.0, 0.14],
orientation=[0.0, 0.0, 0.0, 1.0],
)
)
# lidar.add_depth_data_to_frame()
# lidar.add_point_cloud_data_to_frame()
# lidar.set_rotation_frequency(0)
# lidar.set_resolution([0.4, 2])
# # lidar.enable_visualization()
# lidar.prim.GetAttribute("highLod").Set(True)
# lidar.prim.GetAttribute("highLod").Set(True)
lidar.add_depth_data_to_frame()
lidar.add_point_cloud_data_to_frame()
lidar.set_rotation_frequency(0)
lidar.set_resolution([0.4, 2])
# lidar.enable_visualization()
lidar.prim.GetAttribute("highLod").Set(True)
lidar.prim.GetAttribute("highLod").Set(True)
# lidar_data_pipe = NumpyMemMapDataPipe(
# "lidar_data_pipe", force=True, dtype="float32", shape=(900, 16, 3)
# )
lidar_data_pipe = NumpyMemMapDataPipe(
"lidar_data_pipe", force=True, dtype="float32", shape=(900, 16, 3)
)
world.reset()
go2.initialize()
breakpoint()
# Add cameras
# Add cameras if required
print("Adding cameras")
ann = AnnotatorManager(world)
@ -140,24 +140,7 @@ for camera in cameras:
)
camera_pipes[camera["name"] + "_" + type] = pipe
# Store simulation hyperparamters in shared memory
print("Storing simulation hyperparamters in shared memory")
meta_data = {
"camera_names": [camera["name"] for camera in cameras],
"camera_types": [camera["type"] for camera in cameras],
"camera_resolutions": [camera["resolution"] for camera in cameras],
"camera_intrinsics": [
ann.getCameraIntrinsics(camera["name"]) for camera in cameras
],
"camera_extrinsics": [
ann.getCameraExtrinsics(camera["name"]) for camera in cameras
],
}
with open("/dev/shm/fr3_sim_meta_data.pkl", "wb") as f:
pickle.dump(meta_data, f)
lcm_server = LCMBridgeServer(robot_name="b1")
# lcm_server = LCMBridgeServer(robot_name="b1")
cmd_stamp = time.time()
cmd_stamp_old = cmd_stamp
@ -165,7 +148,14 @@ cmd = UnitreeLowCommand()
cmd.kd = 12 * [2.5]
cmd.kp = 12 * [100]
cmd.dq_des = 12 * [0]
cmd.q_des = b1.init_joint_pos
cmd.q_des = go2.init_joint_pos
world.reset()
go2.initialize()
# while simulation_app.is_running():
# world.step(render=True)
# breakpoint()
# sim_manager = simulationManager(
# robot=go2,
@ -181,6 +171,8 @@ while simulation_app.is_running():
# sim_manager.step(counter * PHYSICS_DT)
if counter % 4 == 0:
world.step(render=True)
if robots[0]["lidar"]:
pc = lidar.get_current_frame()["point_cloud"]
lidar_data_pipe.write(pc, match_length=True)

19
Go2Py/sim/isaacsim/sim_config.yaml
View File

@ -13,16 +13,17 @@ robots:
usd_file: "assets/usd/go2.usd"
position: [0.0, 0.0, 0.8]
orientation: [0.0, 0.0, 0.0, 1.0]
lidar: False
cameras:
- prim_path: "/World/Env/GO2/imu_link"
name: "test_camera"
type: ["rgb", "distance_to_camera"]
resolution: [1280, 720]
interstice_parameters:
K: [1000, 1000, 500, 500]
D: [0.1, 0.2, 0.1, 0.0]
translation: [0.0, 0.0, 0.0] # x, y, z
orientation: [0.0, 0.0, 0.0, 1.0] # qx, qy, qz, qw
# - prim_path: "/World/Env/GO2/Go2/base/base"
# name: "test_camera"
# type: ["rgb", "distance_to_camera"]
# resolution: [1280, 720]
# interstice_parameters:
# K: [1000, 1000, 500, 500]
# D: [0.1, 0.2, 0.1, 0.0]
# translation: [0.0, 0.0, 0.0] # x, y, z
# orientation: [0.0, 0.0, 0.0, 1.0] # qx, qy, qz, qw

407
pin_model.ipynb Normal file
View File

@ -0,0 +1,407 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import pinocchio as pin"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"dir(pin)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import os\n",
"from Go2Py import ASSETS_PATH\n",
"urdf_path = os.path.join(ASSETS_PATH, 'urdf/go2.urdf')\n",
"urdf_root_path = os.path.join(ASSETS_PATH, 'urdf')"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot = pin.RobotWrapper.BuildFromURDF(urdf_path, urdf_root_path, pin.JointModelFreeFlyer())"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot_model = robot.model\n",
"robot_data = robot.data"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot_mass = sum([I.mass for I in robot_model.inertias])\n",
"base_frame_name = robot_model.frames[2].name"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"for frame in robot.model.frames:\n",
" print(frame.name)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"for name in robot.model.names:\n",
" print(name)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot_model.getJointId(\"RR_calf_joint\")"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"unitree_joints = [\n",
" 'FR_hip_joint',\n",
" 'FR_thigh_joint',\n",
" 'FR_calf_joint',\n",
" 'FL_hip_joint',\n",
" 'FL_thigh_joint',\n",
" 'FL_calf_joint',\n",
" 'RR_hip_joint',\n",
" 'RR_thigh_joint',\n",
" 'RR_calf_joint',\n",
" 'RL_hip_joint',\n",
" 'RL_thigh_joint',\n",
" 'RL_calf_joint',\n",
" ]\n",
"pin_joints = [name for name in robot.model.names[2:]]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"pin_unitree_id_translator = [robot.model.getJointId(joint_name) for joint_name in unitree_joints]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"pin_unitree_id_translator"
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {},
"outputs": [],
"source": [
"import os\n",
"from Go2Py import ASSETS_PATH\n",
"import pinocchio as pin\n",
"import numpy as np\n",
"urdf_path = os.path.join(ASSETS_PATH, 'urdf/go2.urdf')\n",
"urdf_root_path = os.path.join(ASSETS_PATH, 'urdf')\n",
"\n",
"\n",
"class Go2Model:\n",
" def __init__(self):\n",
" self.robot = pin.RobotWrapper.BuildFromURDF(urdf_path, urdf_root_path, pin.JointModelFreeFlyer())\n",
" # Standing joint configuration in Unitree Joint order\n",
" self.ef_frames = ['FR_foot', 'FL_foot', 'RR_foot', 'RL_foot']\n",
" self.dq_reordering_idx = np.array([0, 1, 2, 3, 4, 5,\\\n",
" 9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14])\n",
" self.q_reordering_idx = np.array([9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14])-6\n",
" self.ef_J_ = {}\n",
" \n",
" def update(self, q, dq, T, v):\n",
" q_ = np.hstack([pin.SE3ToXYZQUATtuple(pin.SE3(T)), q[self.q_reordering_idx]])\n",
" dq_ = np.hstack([v, dq[self.q_reordering_idx]])\n",
" self.robot.computeJointJacobians(q_)\n",
" self.robot.framesForwardKinematics(q_)\n",
" self.robot.centroidalMomentum(q_,dq_)\n",
" self.nle_ = self.robot.nle(q_, dq_)[self.dq_reordering_idx]\n",
" self.g_ = self.robot.gravity(q_)[self.dq_reordering_idx]\n",
" self.M_ = self.robot.mass(q_)[[self.dq_reordering_idx],[self.dq_reordering_idx]]\n",
" self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q_)[[self.dq_reordering_idx],[self.dq_reordering_idx]]\n",
" for ef_frame in self.ef_frames:\n",
" J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL)\n",
" self.ef_J_[ef_frame]=J[:, self.dq_reordering_idx]\n",
"\n",
" def getInfo(self):\n",
" return {\n",
" 'M':self.M_,\n",
" 'Minv':self.Minv_,\n",
" 'nle':self.nle_,\n",
" 'g':self.g_,\n",
" 'J':self.ef_J_,\n",
" }"
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {},
"outputs": [],
"source": [
"model = Go2Model()"
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {},
"outputs": [],
"source": [
"model.update(np.zeros(12), np.zeros(12), np.eye(4), np.zeros(6))"
]
},
{
"cell_type": "code",
"execution_count": 25,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'FR_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , 0.142 , 0. ,\n",
" -0.426 , -0.213 , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 1. , 0. , 0.426 , 0. , 0.1934, 0.426 ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 1. , -0.142 , -0.1934, 0. , -0.0955,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 1. , 0. , 0. , 1. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n",
" 1. , 1. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ]]),\n",
" 'FL_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , -0.142 , 0. ,\n",
" 0. , 0. , 0. , -0.426 , -0.213 , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 1. , 0. , 0.426 , 0. , 0.1934, 0. ,\n",
" 0. , 0. , 0.426 , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 1. , 0.142 , -0.1934, 0. , 0. ,\n",
" 0. , 0. , 0.0955, 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n",
" 0. , 0. , 1. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n",
" 0. , 0. , 0. , 1. , 1. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ]]),\n",
" 'RR_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , 0.142 , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , -0.426 ,\n",
" -0.213 , 0. , 0. , 0. ],\n",
" [ 0. , 1. , 0. , 0.426 , 0. , -0.1934, 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0.426 , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 1. , -0.142 , 0.1934, 0. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , -0.0955, 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n",
" 0. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 1. ,\n",
" 1. , 0. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ]]),\n",
" 'RL_foot': array([[ 1. , 0. , 0. , 0. , -0.426 , -0.142 , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , -0.426 , -0.213 ],\n",
" [ 0. , 1. , 0. , 0.426 , 0. , -0.1934, 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0.426 , 0. , 0. ],\n",
" [ 0. , 0. , 1. , 0.142 , 0.1934, 0. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0.0955, 0. , 0. ],\n",
" [ 0. , 0. , 0. , 1. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 1. , 0. , 0. ],\n",
" [ 0. , 0. , 0. , 0. , 1. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 1. , 1. ],\n",
" [ 0. , 0. , 0. , 0. , 0. , 1. , 0. ,\n",
" 0. , 0. , 0. , 0. , 0. , 0. , 0. ,\n",
" 0. , 0. , 0. , 0. ]])}"
]
},
"execution_count": 25,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"model.getInfo()['J']"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"q = np.hstack([pin.SE3ToXYZQUATtuple(pin.SE3(np.eye(4))), np.zeros(12)])\n",
"v = np.zeros(18)\n",
"robot.computeJointJacobians(q)\n",
"robot.framesForwardKinematics(q)\n",
"robot.updateGeometryPlacements(q)\n",
"nle = robot.nle(q, v)\n",
"g = robot.gravity(q)\n",
"robot.centroidalMomentum(q,v)\n",
"pin.computeMinverse(q)\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot.frameJacobian(q, robot.model.getFrameId('FL_Foot'))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot.model.frames[2]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot.getFrameJacobian(robot.model.getFrameId('FL_Foot'), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)[0:6,0:6]"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"pin.se3ToXYZQUATtuple(pin.SE3(np.eye(4)))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"pin.SE3(np.eye(4))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"pin.rnea(robot.model, robot.data, q, v, v).shape"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"M = pin.crba(robot.model, robot.data, q)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"M.T-M"
]
},
{
"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
}