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

robot model is updated

This commit is contained in:
Rooholla-KhorramBakht 2024-04-13 23:00:01 -04:00
parent b6a24a93d1
commit d91e5ceb38
7 changed files with 553 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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]]

94
Go2Py/robot/model.py
View File

@ -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):

28
Go2Py/robot/remote.py
View File

@ -60,4 +60,30 @@ class KeyboardRemote(BaseRemote):
def flushStates(self):
self.stand_up_down_seq_flag = False
self.start_seq_flag = False
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

73
examples/joystick_test.ipynb Normal file
View File

@ -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
}

266
examples/legged_odometry_test.ipynb Normal file
View File

@ -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
}

39
examples/lowlevel_dds_interface.ipynb
View File

@ -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": {

70
examples/lowlevel_ros2_interface.ipynb
View File

@ -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": {