diff --git a/Go2Py/robot/interface/ros2.py b/Go2Py/robot/interface/ros2.py index 297f627..bb989d5 100644 --- a/Go2Py/robot/interface/ros2.py +++ b/Go2Py/robot/interface/ros2.py @@ -12,7 +12,7 @@ from rclpy.executors import MultiThreadedExecutor from geometry_msgs.msg import TransformStamped from Go2Py.joy import xKeySwitch, xRockerBtn from geometry_msgs.msg import TwistStamped -from Go2Py.msgs.unitree_go.msg import LowState, Go2pyLowCmd +from unitree_go.msg import LowState, Go2pyLowCmd from nav_msgs.msg import Odometry from scipy.spatial.transform import Rotation @@ -114,6 +114,7 @@ class GO2Real(Node): self.running = True self.setCommands = {'lowlevel':self.setCommandsLow, 'highlevel':self.setCommandsHigh}[self.mode] + self.state = None def lowstate_callback(self, msg): """ @@ -150,6 +151,8 @@ class GO2Real(Node): def getJointStates(self): """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]] diff --git a/Go2Py/robot/model.py b/Go2Py/robot/model.py index 9533ef9..1903b6c 100644 --- a/Go2Py/robot/model.py +++ b/Go2Py/robot/model.py @@ -29,7 +29,8 @@ class Go2Model: self.dq_reordering_idx = np.array([0, 1, 2, 3, 4, 5,\ 9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14]) self.q_reordering_idx = np.array([9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14])-6 - self.ef_J_ = {} + self.ef_Jb_ = {} + self.ef_Jw_ = {} ID_FL_HAA = self.robot.model.getFrameId('FL_hip_joint') ID_FR_HAA = self.robot.model.getFrameId('FR_hip_joint') @@ -53,9 +54,11 @@ class Go2Model: self.l1 = np.linalg.norm(self.robot.data.oMf[ID_FR_HAA].translation - self.robot.data.oMf[ID_FR_HFE].translation) self.l2 = np.linalg.norm(self.robot.data.oMf[ID_FR_HFE].translation - self.robot.data.oMf[ID_FR_KFE].translation) self.l3 = np.linalg.norm(self.robot.data.oMf[ID_FR_KFE].translation - self.robot.data.oMf[ID_FR_FOOT].translation) - - print(self.robot.data.oMf[ID_FR_HAA].translation - self.robot.data.oMf[ID_RR_HAA].translation) - + self.M_ = None + self.Minv_ = None + self.nle_ = None + self.g_ = None + # print(self.robot.data.oMf[ID_FR_HAA].translation - self.robot.data.oMf[ID_RR_HAA].translation) # print(self.h) # print(self.b) # print(self.l1) @@ -133,8 +136,69 @@ class Go2Model: return {frame:self.robot.data.oMf[self.robot.model.getFrameId(frame)].homogeneous \ for frame in ef_frames} + def updateKinematics(self, q): + """ + Updates the kinematic states. + + Args: + 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. + """ + self.robot.computeJointJacobians(q) + self.robot.framesForwardKinematics(q)\ + + for ef_frame in self.ef_frames: + Jw = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) + Jb = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL) + self.ef_Jw_[ef_frame]=Jw[:, self.dq_reordering_idx] + self.ef_Jb_[ef_frame]=Jb[:, self.dq_reordering_idx] - def update(self, q, dq, T, v): + def updateKinematicsPose(self, q, T): + """ + Updates the kinematic states. + + Args: + q (np.ndarray): A numpy array of size 12 representing the joint configurations in FR, FL, RR, RL order. + T (np.ndarray): 4x4 Transformation matrix representing the base pose of the robot. + """ + q_ = np.hstack([pin.SE3ToXYZQUATtuple(pin.SE3(T)), q[self.q_reordering_idx]]) + self.robot.computeJointJacobians(q_) + self.robot.framesForwardKinematics(q_)\ + + for ef_frame in self.ef_frames: + Jw = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) + Jb = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL) + self.ef_Jw_[ef_frame]=Jw[:, self.dq_reordering_idx] + self.ef_Jb_[ef_frame]=Jb[:, self.dq_reordering_idx] + + def updateDynamics(self, q, dq): + """ + Updates the dynamical states. + + Args: + 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.M_ = self.M_[:,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] + + + def updateAll(q, dq): + """ + Updates the dynamic and kinematic parameters based on the given joint configurations and velocities. + + Args: + 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.updateKinematics(q) + self.updateDynamics(q, dq) + + def updateAllPose(self, q, dq, T, v): """ Updates the dynamic and kinematic parameters based on the given joint configurations and velocities. @@ -146,20 +210,9 @@ class Go2Model: """ q_ = np.hstack([pin.SE3ToXYZQUATtuple(pin.SE3(T)), q[self.q_reordering_idx]]) dq_ = np.hstack([v, dq[self.q_reordering_idx]]) - self.robot.computeJointJacobians(q_) - self.robot.framesForwardKinematics(q_) - 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_ = self.Minv_[:,self.dq_reordering_idx] - for ef_frame in self.ef_frames: - J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL_WORLD_ALIGNED) - # J = self.robot.getFrameJacobian(self.robot.model.getFrameId(ef_frame), pin.ReferenceFrame.LOCAL) - self.ef_J_[ef_frame]=J[:, self.dq_reordering_idx] - + self.updateKinematics(q_) + self.updateDynamics(q_, dq_) + def getInfo(self): """ Retrieves the current dynamics and kinematic information of the robot. @@ -172,7 +225,8 @@ class Go2Model: 'Minv':self.Minv_, 'nle':self.nle_, 'g':self.g_, - 'J':self.ef_J_, + 'J_w':self.ef_Jw_, + 'J_b':self.ef_Jb_, } def getGroundReactionForce(self, tau_est, body_acceleration=None): diff --git a/Go2Py/robot/remote.py b/Go2Py/robot/remote.py index e6a704e..419d840 100644 --- a/Go2Py/robot/remote.py +++ b/Go2Py/robot/remote.py @@ -60,4 +60,30 @@ class KeyboardRemote(BaseRemote): def flushStates(self): self.stand_up_down_seq_flag = False - self.start_seq_flag = False \ No newline at end of file + self.start_seq_flag = False + +class UnitreeRemote(BaseRemote): + def __init__(self, robot): + self.robot = robot + + def startSeq(self): + remote = self.robot.getRemoteState() + if remote.btn.start: + return True + else: + return False + + def standUpDownSeq(self): + remote = self.robot.getRemoteState() + if remote.btn.L2 and remote.btn.A: + return True + else: + return False + + def getEstop(self): + remote = self.robot.getRemoteState() + if remote.btn.L2 and remote.btn.R2: + return True + else: + return False + \ No newline at end of file diff --git a/examples/joystick_test.ipynb b/examples/joystick_test.ipynb new file mode 100644 index 0000000..c01a456 --- /dev/null +++ b/examples/joystick_test.ipynb @@ -0,0 +1,73 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.robot.interface.dds import GO2Real " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot = GO2Real()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import time\n", + "while True:\n", + " time.sleep(0.1)\n", + " print(robot.getRemoteState())" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.robot.remote import UnitreeRemote" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "remote = UnitreeRemote(robot)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "b1py", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.18" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/examples/legged_odometry_test.ipynb b/examples/legged_odometry_test.ipynb new file mode 100644 index 0000000..97aa105 --- /dev/null +++ b/examples/legged_odometry_test.ipynb @@ -0,0 +1,266 @@ +{ + "cells": [ + { + "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.10)\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.model import Go2Model\n", + "import time\n", + "ros2_init()\n", + "robot = GO2Real(mode='highlevel')\n", + "ros2_exec_manager = ROS2ExecutorManager()\n", + "ros2_exec_manager.add_node(robot)\n", + "ros2_exec_manager.start()" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'q': array([-0.04339516, 1.257864 , -2.80321264, 0.01644808, 1.26709867,\n", + " -2.79090714, -0.36422092, 1.27348173, -2.80375004, 0.34087694,\n", + " 1.27862883, -2.80913687]),\n", + " 'dq': array([-0.02712867, 0.00387552, -0.00202201, -0.02712867, 0.0658839 ,\n", + " -0.02022013, 0.00775105, -0.00775105, 0.01415409, 0.01937762,\n", + " 0.10463915, -0.05055031])}" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "state = robot.getJointStates()\n", + "state" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "import pinocchio as pin\n", + "import numpy as np" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "model = Go2Model()" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'FR_foot': array([[ 2.54449360e-02, 4.33674972e-02, 9.98735108e-01,\n", + " -1.41910262e-01, -2.05117030e-01, 1.25221489e-02,\n", + " -9.54690795e-02, -1.20784475e-02, -2.13000000e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 9.99058578e-01, -4.33815430e-02,\n", + " 7.30090366e-02, 8.83575282e-03, 2.03483648e-01,\n", + " 7.09917949e-02, 7.04731412e-19, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [-9.99676225e-01, 1.10384059e-03, 2.54209816e-02,\n", + " -3.61206704e-03, 6.15827735e-02, -1.44718059e-01,\n", + " -2.42999139e-03, 7.07073528e-02, -1.04083409e-17,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 2.54449360e-02, 4.33674972e-02, 9.98735108e-01,\n", + " 2.54449360e-02, -6.93889390e-18, -6.93889390e-18,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 9.99058578e-01, -4.33815430e-02,\n", + " 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -9.99676225e-01, 1.10384059e-03, 2.54209816e-02,\n", + " -9.99676225e-01, -4.33680869e-19, -4.33680869e-19,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n", + " 'FL_foot': array([[ 4.69705590e-02, -1.64291856e-02, 9.98761157e-01,\n", + " 1.41836988e-01, -2.06048603e-01, -1.00598359e-02,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 9.53945942e-02, -1.29637269e-02, -2.13000000e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 9.99864733e-01, 1.64473389e-02,\n", + " 7.44673272e-02, -3.33736863e-03, 2.02884930e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 7.37025259e-02, 5.42101086e-20, 4.33680869e-19,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [-9.98896274e-01, -7.72540702e-04, 4.69642054e-02,\n", + " 6.66952394e-03, 6.25126142e-02, 1.42884482e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 4.48568838e-03, 7.31743768e-02, 5.20417043e-18,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 4.69705590e-02, -1.64291856e-02, 9.98761157e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 4.69705590e-02, 3.46944695e-18, 3.46944695e-18,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 9.99864733e-01, 1.64473389e-02,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -9.98896274e-01, -7.72540702e-04, 4.69642054e-02,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -9.98896274e-01, 1.08420217e-19, 1.08420217e-19,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n", + " 'RR_foot': array([[ 4.05169215e-02, 3.55928916e-01, 9.33634289e-01,\n", + " -1.38835575e-01, 1.70692580e-01, -5.90479974e-02,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -9.54215804e-02, -1.20404787e-02, -2.13000000e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 9.34401571e-01, -3.56221426e-01,\n", + " 8.75935353e-02, -6.56266297e-02, -1.72144687e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 7.10292389e-02, -4.33680869e-19, 2.77555756e-17,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [-9.99178852e-01, 1.44329956e-02, 3.78590751e-02,\n", + " -5.62981300e-03, 3.92988943e-02, -1.63564240e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -3.86936601e-03, 7.05993683e-02, -1.56125113e-17,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 4.05169215e-02, 3.55928916e-01, 9.33634289e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 4.05169215e-02, -5.55111512e-17, -5.55111512e-17,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 9.34401571e-01, -3.56221426e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -9.99178852e-01, 1.44329956e-02, 3.78590751e-02,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -9.99178852e-01, 2.60208521e-17, 2.60208521e-17,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n", + " 'RL_foot': array([[ 4.02773873e-02, -3.34042417e-01, 9.41697083e-01,\n", + " 1.39211420e-01, 1.72418133e-01, 5.52066037e-02,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 9.54225053e-02, -1.16630895e-02, -2.13000000e-01],\n", + " [ 0.00000000e+00, 9.42461856e-01, 3.34313700e-01,\n", + " 8.54747509e-02, 6.16963531e-02, -1.73927839e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 6.99291638e-02, 1.12757026e-17, 0.00000000e+00],\n", + " [-9.99188537e-01, -1.34652824e-02, 3.79599012e-02,\n", + " 5.61162590e-03, 4.09563977e-02, 1.62238613e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 3.84649049e-03, 6.95158146e-02, -6.93889390e-18],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 4.02773873e-02, -3.34042417e-01, 9.41697083e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 4.02773873e-02, -5.55111512e-17, -5.55111512e-17],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 9.42461856e-01, 3.34313700e-01,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 1.00000000e+00, 1.00000000e+00],\n", + " [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -9.99188537e-01, -1.34652824e-02, 3.79599012e-02,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n", + " -9.99188537e-01, -5.20417043e-18, -5.20417043e-18]])}" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "model.updateKinematicsPose(state['q'], np.eye(4))\n", + "model.getInfo()['Jb']" + ] + }, + { + "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": 4 +} diff --git a/examples/lowlevel_dds_interface.ipynb b/examples/lowlevel_dds_interface.ipynb index 0ffb68b..18b8c76 100644 --- a/examples/lowlevel_dds_interface.ipynb +++ b/examples/lowlevel_dds_interface.ipynb @@ -79,6 +79,45 @@ "source": [ "Rotation.from_matrix(np.eye(3)).as_quat()" ] + }, + { + "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" + ] + }, + { + "ename": "NotImplementedError", + "evalue": "DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mNotImplementedError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[1], line 3\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mGo2Py\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01minterface\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mdds\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m GO2Real\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mtime\u001b[39;00m\n\u001b[0;32m----> 3\u001b[0m robot \u001b[38;5;241m=\u001b[39m \u001b[43mGO2Real\u001b[49m\u001b[43m(\u001b[49m\u001b[43mmode\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mhighlevel\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m)\u001b[49m\n", + "File \u001b[0;32m~/projects/rooholla/locomotion/Go2Py/Go2Py/robot/interface/dds.py:35\u001b[0m, in \u001b[0;36mGO2Real.__init__\u001b[0;34m(self, mode, vx_max, vy_max, ωz_max)\u001b[0m\n\u001b[1;32m 33\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmode \u001b[38;5;241m=\u001b[39m mode\n\u001b[1;32m 34\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmode \u001b[38;5;241m==\u001b[39m \u001b[38;5;124m'\u001b[39m\u001b[38;5;124mhighlevel\u001b[39m\u001b[38;5;124m'\u001b[39m:\n\u001b[0;32m---> 35\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m \u001b[38;5;167;01mNotImplementedError\u001b[39;00m(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mDDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.\u001b[39m\u001b[38;5;124m'\u001b[39m)\n\u001b[1;32m 36\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msimulated \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mFalse\u001b[39;00m\n\u001b[1;32m 37\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mprestanding_q \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39marray([ \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.26186061\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.5\u001b[39m,\n\u001b[1;32m 38\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.25883281\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.5\u001b[39m,\n\u001b[1;32m 39\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.27193761\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.6\u001b[39m, \n\u001b[1;32m 40\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.27148342\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.6\u001b[39m])\n", + "\u001b[0;31mNotImplementedError\u001b[0m: DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface." + ] + } + ], + "source": [ + "from Go2Py.robot.interface.dds import GO2Real\n", + "import time\n", + "robot = GO2Real(mode='highlevel')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": { diff --git a/examples/lowlevel_ros2_interface.ipynb b/examples/lowlevel_ros2_interface.ipynb index 2450d51..28a9431 100644 --- a/examples/lowlevel_ros2_interface.ipynb +++ b/examples/lowlevel_ros2_interface.ipynb @@ -1,5 +1,12 @@ { "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Lowlevel Interface Test" + ] + }, { "cell_type": "code", "execution_count": 1, @@ -51,6 +58,69 @@ " robot.setCommands(q, dq, kp, kd, tau)\n", " time.sleep(0.01) " ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Highlevel 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.10)\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", + "import time\n", + "ros2_init()\n", + "robot = GO2Real(mode='highlevel')\n", + "ros2_exec_manager = ROS2ExecutorManager()\n", + "ros2_exec_manager.add_node(robot)\n", + "ros2_exec_manager.start()" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "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" + } + ], + "source": [ + "robot.getJointStates()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": {