go2py bridge bug fix
This commit is contained in:
Rooholla-KhorramBakht
parent
29a9d700d2
commit
97efd0d426
|
|
@ -72,7 +72,7 @@ class GO2Real():
|
|||
Retrieve the state of the robot
|
||||
"""
|
||||
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
|
||||
|
||||
def getIMU(self):
|
||||
|
|
|
|||
|
|
@ -15,6 +15,9 @@ from geometry_msgs.msg import TwistStamped
|
|||
from unitree_go.msg import LowState, Go2pyLowCmd
|
||||
from nav_msgs.msg import Odometry
|
||||
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
|
||||
rpy = self.state.imu_state.rpy
|
||||
temp = self.state.imu_state.temperature
|
||||
# return accel, gyro, quat, temp
|
||||
return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp}
|
||||
|
||||
def getFootContacts(self):
|
||||
|
|
@ -153,13 +157,16 @@ class GO2Real(Node):
|
|||
"""Returns the joint angles (q) and velocities (dq) of the robot"""
|
||||
if self.state is None:
|
||||
return None
|
||||
motorStates = self.state.motor_state
|
||||
_q, _dq = zip(
|
||||
*[(motorState.q, motorState.dq) for motorState in motorStates[:12]]
|
||||
)
|
||||
q, dq = np.array(_q), np.array(_dq)
|
||||
|
||||
return {'q':q, 'dq':dq}
|
||||
motor_state = np.array([[self.state.motor_state[i].q,
|
||||
self.state.motor_state[i].dq,
|
||||
self.state.motor_state[i].ddq,
|
||||
self.state.motor_state[i].tau_est,
|
||||
self.state.motor_state[i].temperature] for i in range(12)])
|
||||
return {'q':motor_state[:,0],
|
||||
'dq':motor_state[:,1],
|
||||
'ddq':motor_state[:,2],
|
||||
'tau_est':motor_state[:,3],
|
||||
'temperature':motor_state[:,4]}
|
||||
|
||||
def getRemoteState(self):
|
||||
"""A method to get the state of the wireless remote control.
|
||||
|
|
@ -224,13 +231,13 @@ class GO2Real(Node):
|
|||
self.highcmd_publisher.publish(self.highcmd)
|
||||
|
||||
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.q = q_des.tolist()
|
||||
lowcmd.dq = dq_des.tolist()
|
||||
lowcmd.kp = kp.tolist()
|
||||
lowcmd.kd = kd.tolist()
|
||||
lowcmd.tau = tau_ff.tolist()
|
||||
lowcmd.q = q_des
|
||||
lowcmd.dq = dq_des
|
||||
lowcmd.kp = kp
|
||||
lowcmd.kd = kd
|
||||
lowcmd.tau = tau_ff
|
||||
self.lowcmd_publisher.publish(lowcmd)
|
||||
self.latest_command_stamp = time.time()
|
||||
|
||||
|
|
@ -264,3 +271,43 @@ class GO2Real(Node):
|
|||
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
|
||||
|
||||
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()
|
||||
|
|
@ -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.
|
||||
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.nle_ = self.robot.nle(q_, dq_)[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.robot.centroidalMomentum(q,dq)
|
||||
self.nle_ = self.robot.nle(q, dq)[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.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]
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -40,22 +40,22 @@ class Custom: public rclcpp::Node
|
|||
|
||||
// Go2 highlevel subscriber and publishers
|
||||
// the state_suber is set to subscribe "sportmodestate" topic
|
||||
highstate_suber = this->create_subscription<unitree_go::msg::SportModeState>(
|
||||
"sportmodestate", 10, std::bind(&Custom::highstate_callback, this, std::placeholders::_1));
|
||||
// highstate_suber = this->create_subscription<unitree_go::msg::SportModeState>(
|
||||
// "sportmodestate", 10, std::bind(&Custom::highstate_callback, this, std::placeholders::_1));
|
||||
// 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
|
||||
init_lowcmd();
|
||||
lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
|
||||
"lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1));
|
||||
// lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
|
||||
// "lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1));
|
||||
|
||||
lowcmd_suber = this->create_subscription<unitree_go::msg::Go2pyLowCmd>(
|
||||
"/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1));
|
||||
|
||||
lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/lowcmd", 10);
|
||||
api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 10);
|
||||
status_publisher = this->create_publisher<unitree_go::msg::Go2pyStatus>("/go2py/status", 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", 1);
|
||||
status_publisher = this->create_publisher<unitree_go::msg::Go2pyStatus>("/go2py/status", 1);
|
||||
|
||||
}
|
||||
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].tau = data->tau[i]; // Feedforward toque 1N.m
|
||||
get_crc(lowcmd_msg); //Compute the CRC and load it into the message
|
||||
lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message
|
||||
}
|
||||
}else
|
||||
{
|
||||
|
|
@ -323,10 +322,10 @@ 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].tau = 0.; // Feedforward toque 1N.m
|
||||
get_crc(lowcmd_msg); //Compute the CRC and load it into the message
|
||||
}
|
||||
}
|
||||
lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Custom::watchdog()
|
||||
{
|
||||
|
|
|
|||
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -2761,18 +2761,186 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 26,
|
||||
"execution_count": 28,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"ename": "TypeError",
|
||||
"evalue": "__init__() got an unexpected keyword argument 'func'",
|
||||
"output_type": "error",
|
||||
"traceback": [
|
||||
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
|
||||
"\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)",
|
||||
"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",
|
||||
"\u001b[0;31mTypeError\u001b[0m: __init__() got an unexpected keyword argument 'func'"
|
||||
"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"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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) -> numpy.ndarray</span>\n",
|
||||
"<span style=\"color: #bbbbbb\"> </span><span style=\"color: #BA2121; font-style: italic\">"""</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\"> """</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\">"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\">"</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\">"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\">"</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",
|
||||
")\n",
|
||||
"\n",
|
||||
"print(\"Files generated in {}:\\n\".format(double_pendulum_python_data.output_dir))\n",
|
||||
"for f in double_pendulum_python_data.generated_files:\n",
|
||||
" print(\" |- {}\".format(f.relative_to(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",
|
||||
"# print(\" |- {}\".format(f.relative_to(double_pendulum_python_data.output_dir)))\n",
|
||||
"\n",
|
||||
"display_code_file(\n",
|
||||
" double_pendulum_python_data.function_dir / \"double_pendulum.py\",\n",
|
||||
|
|
|
|||
|
|
@ -9,23 +9,14 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"execution_count": null,
|
||||
"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"
|
||||
]
|
||||
}
|
||||
],
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
|
||||
"import time\n",
|
||||
"ros2_init()\n",
|
||||
"robot = GO2Real(mode='lowlevel')\n",
|
||||
"robot = GO2Real(mode='highlevel')\n",
|
||||
"ros2_exec_manager = ROS2ExecutorManager()\n",
|
||||
"ros2_exec_manager.add_node(robot)\n",
|
||||
"ros2_exec_manager.start()"
|
||||
|
|
@ -37,12 +28,52 @@
|
|||
"metadata": {},
|
||||
"outputs": [],
|
||||
"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",
|
||||
"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": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
|
|
@ -59,6 +90,16 @@
|
|||
" 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",
|
||||
"metadata": {},
|
||||
|
|
@ -68,18 +109,9 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"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"
|
||||
]
|
||||
}
|
||||
],
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
|
||||
"import time\n",
|
||||
|
|
@ -92,25 +124,9 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 2,
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"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"
|
||||
}
|
||||
],
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"robot.getJointStates()"
|
||||
]
|
||||
|
|
@ -139,7 +155,7 @@
|
|||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.10"
|
||||
"version": "3.8.18"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,4 @@
|
|||
from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager
|
||||
import time
|
||||
ros2_init()
|
||||
robot = GO2Real(mode='highlevel')
|
||||
Loading…
Reference in New Issue