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

go2py bridge bug fix

This commit is contained in:
Rooholla-KhorramBakht 2024-04-30 22:32:27 -04:00
parent 29a9d700d2
commit 97efd0d426
20 changed files with 920 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Go2Py/robot/interface/dds.py
View File

@ -72,7 +72,7 @@ class GO2Real():
Retrieve the state of the robot Retrieve the state of the robot
""" """
while self.running: while self.running:
for msg in self.lowstate_reader.take_iter(timeout=duration(milliseconds=100.)): for msg in self.lowstate_reader.take_iter(timeout=duration(milliseconds=1.)):
self.state = msg self.state = msg
def getIMU(self): def getIMU(self):

75
Go2Py/robot/interface/ros2.py
View File

@ -15,6 +15,9 @@ from geometry_msgs.msg import TwistStamped
from unitree_go.msg import LowState, Go2pyLowCmd from unitree_go.msg import LowState, Go2pyLowCmd
from nav_msgs.msg import Odometry from nav_msgs.msg import Odometry
from scipy.spatial.transform import Rotation from scipy.spatial.transform import Rotation
import tf2_ros
import numpy as np
from scipy.spatial.transform import Rotation as R
@ -142,6 +145,7 @@ class GO2Real(Node):
quat = self.state.imu_state.quaternion quat = self.state.imu_state.quaternion
rpy = self.state.imu_state.rpy rpy = self.state.imu_state.rpy
temp = self.state.imu_state.temperature temp = self.state.imu_state.temperature
# return accel, gyro, quat, temp
return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp} return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp}
def getFootContacts(self): def getFootContacts(self):
@ -153,13 +157,16 @@ class GO2Real(Node):
"""Returns the joint angles (q) and velocities (dq) of the robot""" """Returns the joint angles (q) and velocities (dq) of the robot"""
if self.state is None: if self.state is None:
return None return None
motorStates = self.state.motor_state motor_state = np.array([[self.state.motor_state[i].q,
_q, _dq = zip( self.state.motor_state[i].dq,
*[(motorState.q, motorState.dq) for motorState in motorStates[:12]] self.state.motor_state[i].ddq,
) self.state.motor_state[i].tau_est,
q, dq = np.array(_q), np.array(_dq) self.state.motor_state[i].temperature] for i in range(12)])
return {'q':motor_state[:,0],
return {'q':q, 'dq':dq} 'dq':motor_state[:,1],
'ddq':motor_state[:,2],
'tau_est':motor_state[:,3],
'temperature':motor_state[:,4]}
def getRemoteState(self): def getRemoteState(self):
"""A method to get the state of the wireless remote control. """A method to get the state of the wireless remote control.
@ -224,13 +231,13 @@ class GO2Real(Node):
self.highcmd_publisher.publish(self.highcmd) self.highcmd_publisher.publish(self.highcmd)
def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff): def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff):
assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12" # assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
lowcmd = Go2pyLowCmd() lowcmd = Go2pyLowCmd()
lowcmd.q = q_des.tolist() lowcmd.q = q_des
lowcmd.dq = dq_des.tolist() lowcmd.dq = dq_des
lowcmd.kp = kp.tolist() lowcmd.kp = kp
lowcmd.kd = kd.tolist() lowcmd.kd = kd
lowcmd.tau = tau_ff.tolist() lowcmd.tau = tau_ff
self.lowcmd_publisher.publish(lowcmd) self.lowcmd_publisher.publish(lowcmd)
self.latest_command_stamp = time.time() self.latest_command_stamp = time.time()
@ -263,4 +270,44 @@ class GO2Real(Node):
q = self.getIMU()['quat'] q = self.getIMU()['quat']
R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix() 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) g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
return g_in_body return g_in_body
class ROS2TFInterface(Node):
def __init__(self, parent_name, child_name, node_name):
super().__init__(f'{node_name}_tf2_listener')
self.parent_name = parent_name
self.child_name = child_name
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer, self)
self.T = None
self.stamp = None
self.running = True
self.thread = threading.Thread(target=self.update_loop)
self.thread.start()
self.trans = None
def update_loop(self):
while self.running:
try:
self.trans = self.tfBuffer.lookup_transform(self.parent_name, self.child_name, rclpy.time.Time(), rclpy.time.Duration(seconds=0.1))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
pass
time.sleep(0.01)
def get_pose(self):
if self.trans is None:
return None
else:
translation = [self.trans.transform.translation.x, self.trans.transform.translation.y, self.trans.transform.translation.z]
rotation = [self.trans.transform.rotation.x, self.trans.transform.rotation.y, self.trans.transform.rotation.z, self.trans.transform.rotation.w]
self.T = np.eye(4)
self.T[0:3, 0:3] = R.from_quat(rotation).as_matrix()
self.T[:3, 3] = translation
self.stamp = self.trans.header.stamp.nanosec * 1e-9 + self.trans.header.stamp.sec
return self.T
def close(self):
self.running = False
self.thread.join()
self.destroy_node()

10
Go2Py/robot/model.py
View File

@ -178,12 +178,12 @@ class Go2Model:
q (np.ndarray): A numpy array of size 19 representing the [x, y, z, qx, qy, qz, qw] and joint configurations in FR, FL, RR, RL order. q (np.ndarray): A numpy array of size 19 representing the [x, y, z, qx, qy, qz, qw] and joint configurations in FR, FL, RR, RL order.
dq (np.ndarray): A numpy array of size 18 representing the [vx, vy, vz, wx, wy, wz] and joint configurations in FR, FL, RR, RL order. dq (np.ndarray): A numpy array of size 18 representing the [vx, vy, vz, wx, wy, wz] and joint configurations in FR, FL, RR, RL order.
""" """
self.robot.centroidalMomentum(q_,dq_) self.robot.centroidalMomentum(q,dq)
self.nle_ = self.robot.nle(q_, dq_)[self.dq_reordering_idx] self.nle_ = self.robot.nle(q, dq)[self.dq_reordering_idx]
self.g_ = self.robot.gravity(q_)[self.dq_reordering_idx] self.g_ = self.robot.gravity(q)[self.dq_reordering_idx]
self.M_ = self.robot.mass(q_)[self.dq_reordering_idx,:] self.M_ = self.robot.mass(q)[self.dq_reordering_idx,:]
self.M_ = self.M_[:,self.dq_reordering_idx] self.M_ = self.M_[:,self.dq_reordering_idx]
self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q_)[self.dq_reordering_idx,:] self.Minv_ = pin.computeMinverse(self.robot.model, self.robot.data, q)[self.dq_reordering_idx,:]
self.Minv_ = self.Minv_[:,self.dq_reordering_idx] self.Minv_ = self.Minv_[:,self.dq_reordering_idx]

19
deploy/robot_ws/src/go2py_node/src/bridge.cpp
View File

@ -40,22 +40,22 @@ class Custom: public rclcpp::Node
// Go2 highlevel subscriber and publishers // Go2 highlevel subscriber and publishers
// the state_suber is set to subscribe "sportmodestate" topic // the state_suber is set to subscribe "sportmodestate" topic
highstate_suber = this->create_subscription<unitree_go::msg::SportModeState>( // highstate_suber = this->create_subscription<unitree_go::msg::SportModeState>(
"sportmodestate", 10, std::bind(&Custom::highstate_callback, this, std::placeholders::_1)); // "sportmodestate", 10, std::bind(&Custom::highstate_callback, this, std::placeholders::_1));
// the req_puber is set to subscribe "/api/sport/request" topic with dt // the req_puber is set to subscribe "/api/sport/request" topic with dt
highreq_puber = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 10); highreq_puber = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 1);
//Go2 lowlevel interface //Go2 lowlevel interface
init_lowcmd(); init_lowcmd();
lowstate_suber = this->create_subscription<unitree_go::msg::LowState>( // lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
"lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1)); // "lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1));
lowcmd_suber = this->create_subscription<unitree_go::msg::Go2pyLowCmd>( lowcmd_suber = this->create_subscription<unitree_go::msg::Go2pyLowCmd>(
"/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1)); "/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1));
lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/lowcmd", 10); lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/lowcmd", 1);
api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 10); api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 1);
status_publisher = this->create_publisher<unitree_go::msg::Go2pyStatus>("/go2py/status", 10); status_publisher = this->create_publisher<unitree_go::msg::Go2pyStatus>("/go2py/status", 1);
} }
private: private:
@ -311,7 +311,6 @@ void Custom::lowcmd_callback(unitree_go::msg::Go2pyLowCmd::SharedPtr data)
lowcmd_msg.motor_cmd[i].kd = data->kd[i]; // Poinstion(rad) control kd gain lowcmd_msg.motor_cmd[i].kd = data->kd[i]; // Poinstion(rad) control kd gain
lowcmd_msg.motor_cmd[i].tau = data->tau[i]; // Feedforward toque 1N.m lowcmd_msg.motor_cmd[i].tau = data->tau[i]; // Feedforward toque 1N.m
get_crc(lowcmd_msg); //Compute the CRC and load it into the message get_crc(lowcmd_msg); //Compute the CRC and load it into the message
lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message
} }
}else }else
{ {
@ -323,9 +322,9 @@ void Custom::lowcmd_callback(unitree_go::msg::Go2pyLowCmd::SharedPtr data)
lowcmd_msg.motor_cmd[i].kd = 0; // Poinstion(rad) control kd gain lowcmd_msg.motor_cmd[i].kd = 0; // Poinstion(rad) control kd gain
lowcmd_msg.motor_cmd[i].tau = 0.; // Feedforward toque 1N.m lowcmd_msg.motor_cmd[i].tau = 0.; // Feedforward toque 1N.m
get_crc(lowcmd_msg); //Compute the CRC and load it into the message get_crc(lowcmd_msg); //Compute the CRC and load it into the message
lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message
} }
} }
lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message
} }
void Custom::watchdog() void Custom::watchdog()

460
examples/compliance_test.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

BIN
examples/data/compliance-test/go2_impact_test2_250.0.pkl Normal file
View File

Binary file not shown.

BIN
examples/data/compliance-test/go2_impact_test2_350.0.pkl Normal file
View File

Binary file not shown.

BIN
examples/data/compliance-test/go2_impact_test2_450.0.pkl Normal file
View File

Binary file not shown.

BIN
examples/data/compliance-test/go2_impact_test2_550.0.pkl Normal file
View File

Binary file not shown.

BIN
examples/data/compliance-test/go2_impact_test2_650.0.pkl Normal file
View File

Binary file not shown.

BIN
examples/data/compliance-test/go2_impact_test2_650.pkl Normal file
View File

Binary file not shown.

BIN
examples/go2_rotation.pkl Normal file
View File

Binary file not shown.

BIN
examples/go2_walking.pkl Normal file
View File

Binary file not shown.

192
examples/legged_inertial_ekf.ipynb
View File

@ -2761,18 +2761,186 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 26, "execution_count": 28,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
"ename": "TypeError", "name": "stderr",
"evalue": "__init__() got an unexpected keyword argument 'func'", "output_type": "stream",
"output_type": "error", "text": [
"traceback": [ "codegen.__init__():141 WARNING -- \n",
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", " Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n",
"\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)", " or numerically unstable results from calling generated functions near singularities.\n",
"Cell \u001b[0;32mIn[26], line 2\u001b[0m\n\u001b[1;32m 1\u001b[0m namespace \u001b[38;5;241m=\u001b[39m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mdouble_pendulum\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[0;32m----> 2\u001b[0m double_pendulum_python \u001b[38;5;241m=\u001b[39m \u001b[43mcodegen\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mCodegen\u001b[49m\u001b[43m(\u001b[49m\n\u001b[1;32m 3\u001b[0m \u001b[43m \u001b[49m\u001b[43mfunc\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mcompute_mean\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m 4\u001b[0m \u001b[43m \u001b[49m\u001b[43mconfig\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43mcodegen\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mPythonConfig\u001b[49m\u001b[43m(\u001b[49m\u001b[43muse_eigen_types\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;28;43;01mFalse\u001b[39;49;00m\u001b[43m)\u001b[49m\u001b[43m,\u001b[49m\n\u001b[1;32m 5\u001b[0m \u001b[43m \u001b[49m\u001b[43mname\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[38;5;124;43mdouble_pendulum\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[43m,\u001b[49m\n\u001b[1;32m 6\u001b[0m \u001b[43m \u001b[49m\u001b[43mreturn_key\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[38;5;124;43mddang\u001b[39;49m\u001b[38;5;124;43m\"\u001b[39;49m\u001b[43m,\u001b[49m\n\u001b[1;32m 7\u001b[0m \u001b[43m)\u001b[49m\n\u001b[1;32m 8\u001b[0m double_pendulum_python_data \u001b[38;5;241m=\u001b[39m double_pendulum_python\u001b[38;5;241m.\u001b[39mgenerate_function(\n\u001b[1;32m 9\u001b[0m namespace\u001b[38;5;241m=\u001b[39mnamespace,\n\u001b[1;32m 10\u001b[0m )\n\u001b[1;32m 12\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mFiles generated in \u001b[39m\u001b[38;5;132;01m{}\u001b[39;00m\u001b[38;5;124m:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;241m.\u001b[39mformat(double_pendulum_python_data\u001b[38;5;241m.\u001b[39moutput_dir))\n", "\n",
"\u001b[0;31mTypeError\u001b[0m: __init__() got an unexpected keyword argument 'func'" " In order to safely generate code, you should set epsilon to either a symbol\n",
" (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n",
" this before importing any other code from symforce, e.g. with\n",
"\n",
" import symforce\n",
" symforce.set_epsilon_to_symbol()\n",
"\n",
" or\n",
"\n",
" import symforce\n",
" symforce.set_epsilon_to_number()\n",
"\n",
" For more information on use of epsilon to prevent singularities, take a look at the\n",
" Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n",
"\n"
]
},
{
"data": {
"text/html": [
"<div class=\"highlight\" style=\"background: #f8f8f8\"><pre style=\"line-height: 125%;\"><span></span><span style=\"color: #3D7B7B; font-style: italic\"># -----------------------------------------------------------------------------</span>\n",
"<span style=\"color: #3D7B7B; font-style: italic\"># This file was autogenerated by symforce from template:</span>\n",
"<span style=\"color: #3D7B7B; font-style: italic\"># function/FUNCTION.py.jinja</span>\n",
"<span style=\"color: #3D7B7B; font-style: italic\"># Do NOT modify by hand.</span>\n",
"<span style=\"color: #3D7B7B; font-style: italic\"># -----------------------------------------------------------------------------</span>\n",
"\n",
"<span style=\"color: #3D7B7B; font-style: italic\"># pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument,unused-import</span>\n",
"\n",
"<span style=\"color: #008000; font-weight: bold\">import</span> <span style=\"color: #0000FF; font-weight: bold\">math</span>\n",
"<span style=\"color: #008000; font-weight: bold\">import</span> <span style=\"color: #0000FF; font-weight: bold\">typing</span> <span style=\"color: #008000; font-weight: bold\">as</span> <span style=\"color: #0000FF; font-weight: bold\">T</span>\n",
"\n",
"<span style=\"color: #008000; font-weight: bold\">import</span> <span style=\"color: #0000FF; font-weight: bold\">numpy</span>\n",
"\n",
"<span style=\"color: #008000; font-weight: bold\">import</span> <span style=\"color: #0000FF; font-weight: bold\">sym</span>\n",
"\n",
"\n",
"<span style=\"color: #008000; font-weight: bold\">def</span> <span style=\"color: #0000FF\">double_pendulum</span>(state, u, dT, epsilon):\n",
" <span style=\"color: #3D7B7B; font-style: italic\"># type: (numpy.ndarray, numpy.ndarray, float, float) -&gt; numpy.ndarray</span>\n",
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #BA2121; font-style: italic\">&quot;&quot;&quot;</span>\n",
"<span style=\"color: #BA2121; font-style: italic\"> This function was autogenerated from a symbolic function. Do not modify by hand.</span>\n",
"\n",
"<span style=\"color: #BA2121; font-style: italic\"> Symbolic function: compute_mean</span>\n",
"\n",
"<span style=\"color: #BA2121; font-style: italic\"> Args:</span>\n",
"<span style=\"color: #BA2121; font-style: italic\"> state: Matrix28_1</span>\n",
"<span style=\"color: #BA2121; font-style: italic\"> u: Matrix61</span>\n",
"<span style=\"color: #BA2121; font-style: italic\"> dT: Scalar</span>\n",
"<span style=\"color: #BA2121; font-style: italic\"> epsilon: Scalar</span>\n",
"\n",
"<span style=\"color: #BA2121; font-style: italic\"> Outputs:</span>\n",
"<span style=\"color: #BA2121; font-style: italic\"> res: Matrix28_1</span>\n",
"<span style=\"color: #BA2121; font-style: italic\"> &quot;&quot;&quot;</span>\n",
"\n",
" <span style=\"color: #3D7B7B; font-style: italic\"># Total ops: 92</span>\n",
"\n",
" <span style=\"color: #3D7B7B; font-style: italic\"># Input arrays</span>\n",
" <span style=\"color: #008000; font-weight: bold\">if</span> state<span style=\"color: #666666\">.</span>shape <span style=\"color: #666666\">==</span> (<span style=\"color: #666666\">28</span>,):\n",
" state <span style=\"color: #666666\">=</span> state<span style=\"color: #666666\">.</span>reshape((<span style=\"color: #666666\">28</span>, <span style=\"color: #666666\">1</span>))\n",
" <span style=\"color: #008000; font-weight: bold\">elif</span> state<span style=\"color: #666666\">.</span>shape <span style=\"color: #666666\">!=</span> (<span style=\"color: #666666\">28</span>, <span style=\"color: #666666\">1</span>):\n",
" <span style=\"color: #008000; font-weight: bold\">raise</span> <span style=\"color: #CB3F38; font-weight: bold\">IndexError</span>(\n",
" <span style=\"color: #BA2121\">&quot;state is expected to have shape (28, 1) or (28,); instead had shape </span><span style=\"color: #A45A77; font-weight: bold\">{}</span><span style=\"color: #BA2121\">&quot;</span><span style=\"color: #666666\">.</span>format(\n",
" state<span style=\"color: #666666\">.</span>shape\n",
" )\n",
" )\n",
"\n",
" <span style=\"color: #008000; font-weight: bold\">if</span> u<span style=\"color: #666666\">.</span>shape <span style=\"color: #666666\">==</span> (<span style=\"color: #666666\">6</span>,):\n",
" u <span style=\"color: #666666\">=</span> u<span style=\"color: #666666\">.</span>reshape((<span style=\"color: #666666\">6</span>, <span style=\"color: #666666\">1</span>))\n",
" <span style=\"color: #008000; font-weight: bold\">elif</span> u<span style=\"color: #666666\">.</span>shape <span style=\"color: #666666\">!=</span> (<span style=\"color: #666666\">6</span>, <span style=\"color: #666666\">1</span>):\n",
" <span style=\"color: #008000; font-weight: bold\">raise</span> <span style=\"color: #CB3F38; font-weight: bold\">IndexError</span>(\n",
" <span style=\"color: #BA2121\">&quot;u is expected to have shape (6, 1) or (6,); instead had shape </span><span style=\"color: #A45A77; font-weight: bold\">{}</span><span style=\"color: #BA2121\">&quot;</span><span style=\"color: #666666\">.</span>format(u<span style=\"color: #666666\">.</span>shape)\n",
" )\n",
"\n",
" <span style=\"color: #3D7B7B; font-style: italic\"># Intermediate terms (16)</span>\n",
" _tmp0 <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">12</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>])\n",
" _tmp1 <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">10</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>])\n",
" _tmp2 <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">11</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>])\n",
" _tmp3 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">14</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">4</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp4 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp5 <span style=\"color: #666666\">=</span> _tmp4 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp6 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp7 <span style=\"color: #666666\">=</span> _tmp6 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp8 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">15</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">5</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp9 <span style=\"color: #666666\">=</span> _tmp6 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp10 <span style=\"color: #666666\">=</span> _tmp4 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp11 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>state[<span style=\"color: #666666\">13</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> u[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp12 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">**</span> <span style=\"color: #666666\">2</span>\n",
" _tmp13 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">**</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">-</span> <span style=\"color: #666666\">1</span>\n",
" _tmp14 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
" _tmp15 <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">**</span> <span style=\"color: #666666\">2</span>\n",
"\n",
" <span style=\"color: #3D7B7B; font-style: italic\"># Output terms</span>\n",
" _res <span style=\"color: #666666\">=</span> numpy<span style=\"color: #666666\">.</span>zeros(<span style=\"color: #666666\">28</span>)\n",
" _res[<span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">=</span> _tmp0 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> _tmp1 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">-</span> _tmp2 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1.0</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">1</span>] <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>_tmp0 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> _tmp1 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> _tmp2 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1.0</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">2</span>] <span style=\"color: #666666\">=</span> _tmp0 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">-</span> _tmp1 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> _tmp2 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1.0</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">3</span>] <span style=\"color: #666666\">=</span> <span style=\"color: #666666\">-</span>_tmp0 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">-</span> _tmp1 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">0</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">-</span> _tmp2 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1.0</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">3</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">4</span>] <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">7</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">4</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">5</span>] <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">8</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">5</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">6</span>] <span style=\"color: #666666\">=</span> dT <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">9</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">6</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">7</span>] <span style=\"color: #666666\">=</span> (\n",
" _tmp11 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp12 <span style=\"color: #666666\">-</span> _tmp13)\n",
" <span style=\"color: #666666\">+</span> _tmp3 <span style=\"color: #666666\">*</span> (_tmp5 <span style=\"color: #666666\">-</span> _tmp7)\n",
" <span style=\"color: #666666\">+</span> _tmp8 <span style=\"color: #666666\">*</span> (_tmp10 <span style=\"color: #666666\">+</span> _tmp9)\n",
" <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">7</span>, <span style=\"color: #666666\">0</span>]\n",
" )\n",
" _res[<span style=\"color: #666666\">8</span>] <span style=\"color: #666666\">=</span> (\n",
" _tmp11 <span style=\"color: #666666\">*</span> (_tmp5 <span style=\"color: #666666\">+</span> _tmp7)\n",
" <span style=\"color: #666666\">+</span> _tmp3 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp13 <span style=\"color: #666666\">-</span> _tmp15)\n",
" <span style=\"color: #666666\">+</span> _tmp8 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp14 <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">2</span> <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>] <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">2</span>, <span style=\"color: #666666\">0</span>])\n",
" <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">8</span>, <span style=\"color: #666666\">0</span>]\n",
" )\n",
" _res[<span style=\"color: #666666\">9</span>] <span style=\"color: #666666\">=</span> (\n",
" _tmp11 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp10 <span style=\"color: #666666\">+</span> _tmp9)\n",
" <span style=\"color: #666666\">+</span> _tmp3 <span style=\"color: #666666\">*</span> (_tmp14 <span style=\"color: #666666\">+</span> _tmp6 <span style=\"color: #666666\">*</span> state[<span style=\"color: #666666\">1</span>, <span style=\"color: #666666\">0</span>])\n",
" <span style=\"color: #666666\">+</span> _tmp8 <span style=\"color: #666666\">*</span> (<span style=\"color: #666666\">-</span>_tmp12 <span style=\"color: #666666\">-</span> _tmp15 <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">1</span>)\n",
" <span style=\"color: #666666\">+</span> state[<span style=\"color: #666666\">9</span>, <span style=\"color: #666666\">0</span>]\n",
" <span style=\"color: #666666\">+</span> <span style=\"color: #666666\">9.8</span>\n",
" )\n",
" _res[<span style=\"color: #666666\">10</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">10</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">11</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">11</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">12</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">12</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">13</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">13</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">14</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">14</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">15</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">15</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">16</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">16</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">17</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">17</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">18</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">18</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">19</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">19</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">20</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">20</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">21</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">21</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">22</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">22</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">23</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">23</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">24</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">24</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">25</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">25</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">26</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">26</span>, <span style=\"color: #666666\">0</span>]\n",
" _res[<span style=\"color: #666666\">27</span>] <span style=\"color: #666666\">=</span> state[<span style=\"color: #666666\">27</span>, <span style=\"color: #666666\">0</span>]\n",
" <span style=\"color: #008000; font-weight: bold\">return</span> _res\n",
"</pre></div>\n"
],
"text/plain": [
"<IPython.core.display.HTML object>"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"codegen.__init__():141 WARNING -- \n",
" Generating code with epsilon set to 0 - This is dangerous! You may get NaNs, Infs,\n",
" or numerically unstable results from calling generated functions near singularities.\n",
"\n",
" In order to safely generate code, you should set epsilon to either a symbol\n",
" (recommended) or a small numerical value like `sf.numeric_epsilon`. You should do\n",
" this before importing any other code from symforce, e.g. with\n",
"\n",
" import symforce\n",
" symforce.set_epsilon_to_symbol()\n",
"\n",
" or\n",
"\n",
" import symforce\n",
" symforce.set_epsilon_to_number()\n",
"\n",
" For more information on use of epsilon to prevent singularities, take a look at the\n",
" Epsilon Tutorial: https://symforce.org/tutorials/epsilon_tutorial.html\n",
"\n"
] ]
} }
], ],
@ -2788,9 +2956,9 @@
" namespace=namespace,\n", " namespace=namespace,\n",
")\n", ")\n",
"\n", "\n",
"print(\"Files generated in {}:\\n\".format(double_pendulum_python_data.output_dir))\n", "# print(\"Files generated in {}:\\n\".format(double_pendulum_python_data.output_dir))\n",
"for f in double_pendulum_python_data.generated_files:\n", "# for f in double_pendulum_python_data.generated_files:\n",
" print(\" |- {}\".format(f.relative_to(double_pendulum_python_data.output_dir)))\n", "# print(\" |- {}\".format(f.relative_to(double_pendulum_python_data.output_dir)))\n",
"\n", "\n",
"display_code_file(\n", "display_code_file(\n",
" double_pendulum_python_data.function_dir / \"double_pendulum.py\",\n", " double_pendulum_python_data.function_dir / \"double_pendulum.py\",\n",

104
examples/lowlevel_ros2_interface.ipynb
View File

@ -9,23 +9,14 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": null,
"metadata": {}, "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": [ "source": [
"from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n", "from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
"import time\n", "import time\n",
"ros2_init()\n", "ros2_init()\n",
"robot = GO2Real(mode='lowlevel')\n", "robot = GO2Real(mode='highlevel')\n",
"ros2_exec_manager = ROS2ExecutorManager()\n", "ros2_exec_manager = ROS2ExecutorManager()\n",
"ros2_exec_manager.add_node(robot)\n", "ros2_exec_manager.add_node(robot)\n",
"ros2_exec_manager.start()" "ros2_exec_manager.start()"
@ -37,12 +28,52 @@
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
"robot.getJointStates()" "import time\n",
"import numpy as np\n",
"qs = []\n",
"dqs = []\n",
"taus = []\n",
"stamps = []\n",
"for i in range(5000):\n",
" time.sleep(0.01)\n",
" state = robot.getJointStates()\n",
" q = state['q']\n",
" dq = state['dq']\n",
" tau = state['tau_est']\n",
" stamp = time.time()\n",
" qs.append(q)\n",
" dqs.append(dq)\n",
" taus.append(tau)\n",
" stamps.append(stamp)\n",
"\n",
"q=np.array(qs)\n",
"dq=np.array(dqs)\n",
"tau=np.array(taus)\n",
"stamp=np.array(stamps)"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import pickle\n",
"with open('go2_rotation.pkl', 'wb') as f:\n",
" pickle.dump(\n",
" {\n",
" 'q':q,\n",
" 'dq':dq, \n",
" 'tau':tau, \n",
" 'stamp':stamp\n",
" }\n",
" ,f\n",
" )"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -59,6 +90,16 @@
" time.sleep(0.01) " " time.sleep(0.01) "
] ]
}, },
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import matplotlib.pyplot as plt\n",
"plt.plot(taus)"
]
},
{ {
"cell_type": "markdown", "cell_type": "markdown",
"metadata": {}, "metadata": {},
@ -68,18 +109,9 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [],
{
"name": "stdout",
"output_type": "stream",
"text": [
"pygame 2.5.2 (SDL 2.28.2, Python 3.8.10)\n",
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
]
}
],
"source": [ "source": [
"from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n", "from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
"import time\n", "import time\n",
@ -92,25 +124,9 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 2, "execution_count": null,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [],
{
"data": {
"text/plain": [
"{'q': array([-0.04306209, 1.25783372, -2.80307055, 0.01638752, 1.26682615,\n",
" -2.79090714, -0.3640998 , 1.27342117, -2.8037343 , 0.34069526,\n",
" 1.27865911, -2.80908942]),\n",
" 'dq': array([ 0.06975943, 0.00775105, -0.0323522 , 0.00387552, 0.03875524,\n",
" 0. , -0.03875524, 0.00775105, 0.01819811, 0.04263076,\n",
" 0.04263076, -0.01011006])}"
]
},
"execution_count": 2,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [ "source": [
"robot.getJointStates()" "robot.getJointStates()"
] ]
@ -139,7 +155,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.8.10" "version": "3.8.18"
} }
}, },
"nbformat": 4, "nbformat": 4,

140
examples/vicon2gt-dataset-collector.ipynb Normal file
View File

@ -0,0 +1,140 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Lowlevel Interface Test"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"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.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
"from Go2Py.robot.interface.ros2 import ROS2TFInterface\n",
"import time\n",
"ros2_init()\n",
"vicon = ROS2TFInterface('vicon/World', 'vicon/GO2/GO2', 'vicon')\n",
"robot = GO2Real(mode='highlevel')\n",
"ros2_exec_manager = ROS2ExecutorManager()\n",
"ros2_exec_manager.add_node(robot)\n",
"ros2_exec_manager.add_node(vicon)\n",
"ros2_exec_manager.start()"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"for i in range(1000):\n",
" time.sleep(0.01)\n",
" imu = robot.getIMU()\n",
" accel = imu['accel']\n",
" gyro = imu['gyro']\n",
" q = imu['quat']\n",
" pose = vicon.get_pose()\n",
" stamp = time.time()"
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [],
"source": [
"import time\n",
"import numpy as np\n",
"im = []\n",
"acc = []\n",
"gy = []\n",
"quaternion = []\n",
"pos = []\n",
"stamps = []\n",
"for i in range(2000):\n",
" time.sleep(0.01)\n",
" imu = robot.getIMU()\n",
" accel = imu['accel']\n",
" gyro = imu['gyro']\n",
" q = imu['quat']\n",
" pose = vicon.get_pose()\n",
" stamp = time.time()\n",
" im.append(imu)\n",
" acc.append(accel)\n",
" gy.append(gyro)\n",
" quaternion.append(q)\n",
" pos.append(pose)\n",
" stamps.append(stamp)\n",
"\n",
"imu = np.array(im)\n",
"accel = np.array(acc)\n",
"gyro = np.array(gy)\n",
"q = np.array(quaternion)\n",
"pose = np.array(pos)\n",
"stamp = np.array(stamps)"
]
},
{
"cell_type": "code",
"execution_count": 17,
"metadata": {},
"outputs": [],
"source": [
"import pickle \n",
"with open('vicon2gt_3.pkl', 'wb') as f:\n",
" pickle.dump(\n",
" {\n",
" 'imu':imu,\n",
" 'accel':accel,\n",
" 'gyro':gyro,\n",
" 'q':q,\n",
" 'pose':pose,\n",
" 'stamp':stamp\n",
" }\n",
" ,f\n",
" )"
]
},
{
"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.18"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

BIN
examples/vicon2gt.pkl Normal file
View File

Binary file not shown.

BIN
examples/vicon2gt_1.pkl Normal file
View File

Binary file not shown.

BIN
examples/vicon2gt_3.pkl Normal file
View File

Binary file not shown.

4
examples/vicon2gt_recorder.py Normal file
View File

@ -0,0 +1,4 @@
from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager
import time
ros2_init()
robot = GO2Real(mode='highlevel')