robot model is updated
This commit is contained in:
Rooholla-KhorramBakht
parent
b6a24a93d1
commit
d91e5ceb38
|
|
@ -12,7 +12,7 @@ from rclpy.executors import MultiThreadedExecutor
|
||||||
from geometry_msgs.msg import TransformStamped
|
from geometry_msgs.msg import TransformStamped
|
||||||
from Go2Py.joy import xKeySwitch, xRockerBtn
|
from Go2Py.joy import xKeySwitch, xRockerBtn
|
||||||
from geometry_msgs.msg import TwistStamped
|
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 nav_msgs.msg import Odometry
|
||||||
from scipy.spatial.transform import Rotation
|
from scipy.spatial.transform import Rotation
|
||||||
|
|
||||||
|
|
@ -114,6 +114,7 @@ class GO2Real(Node):
|
||||||
self.running = True
|
self.running = True
|
||||||
self.setCommands = {'lowlevel':self.setCommandsLow,
|
self.setCommands = {'lowlevel':self.setCommandsLow,
|
||||||
'highlevel':self.setCommandsHigh}[self.mode]
|
'highlevel':self.setCommandsHigh}[self.mode]
|
||||||
|
self.state = None
|
||||||
|
|
||||||
def lowstate_callback(self, msg):
|
def lowstate_callback(self, msg):
|
||||||
"""
|
"""
|
||||||
|
|
@ -150,6 +151,8 @@ class GO2Real(Node):
|
||||||
|
|
||||||
def getJointStates(self):
|
def getJointStates(self):
|
||||||
"""Returns the joint angles (q) and velocities (dq) of the robot"""
|
"""Returns the joint angles (q) and velocities (dq) of the robot"""
|
||||||
|
if self.state is None:
|
||||||
|
return None
|
||||||
motorStates = self.state.motor_state
|
motorStates = self.state.motor_state
|
||||||
_q, _dq = zip(
|
_q, _dq = zip(
|
||||||
*[(motorState.q, motorState.dq) for motorState in motorStates[:12]]
|
*[(motorState.q, motorState.dq) for motorState in motorStates[:12]]
|
||||||
|
|
|
||||||
|
|
@ -29,7 +29,8 @@ class Go2Model:
|
||||||
self.dq_reordering_idx = np.array([0, 1, 2, 3, 4, 5,\
|
self.dq_reordering_idx = np.array([0, 1, 2, 3, 4, 5,\
|
||||||
9, 10, 11, 6, 7, 8, 15, 16, 17, 12, 13, 14])
|
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.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_FL_HAA = self.robot.model.getFrameId('FL_hip_joint')
|
||||||
ID_FR_HAA = self.robot.model.getFrameId('FR_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.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.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)
|
self.l3 = np.linalg.norm(self.robot.data.oMf[ID_FR_KFE].translation - self.robot.data.oMf[ID_FR_FOOT].translation)
|
||||||
|
self.M_ = None
|
||||||
print(self.robot.data.oMf[ID_FR_HAA].translation - self.robot.data.oMf[ID_RR_HAA].translation)
|
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.h)
|
||||||
# print(self.b)
|
# print(self.b)
|
||||||
# print(self.l1)
|
# print(self.l1)
|
||||||
|
|
@ -133,8 +136,69 @@ class Go2Model:
|
||||||
return {frame:self.robot.data.oMf[self.robot.model.getFrameId(frame)].homogeneous \
|
return {frame:self.robot.data.oMf[self.robot.model.getFrameId(frame)].homogeneous \
|
||||||
for frame in ef_frames}
|
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.
|
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]])
|
q_ = np.hstack([pin.SE3ToXYZQUATtuple(pin.SE3(T)), q[self.q_reordering_idx]])
|
||||||
dq_ = np.hstack([v, dq[self.q_reordering_idx]])
|
dq_ = np.hstack([v, dq[self.q_reordering_idx]])
|
||||||
self.robot.computeJointJacobians(q_)
|
self.updateKinematics(q_)
|
||||||
self.robot.framesForwardKinematics(q_)
|
self.updateDynamics(q_, dq_)
|
||||||
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]
|
|
||||||
|
|
||||||
def getInfo(self):
|
def getInfo(self):
|
||||||
"""
|
"""
|
||||||
Retrieves the current dynamics and kinematic information of the robot.
|
Retrieves the current dynamics and kinematic information of the robot.
|
||||||
|
|
@ -172,7 +225,8 @@ class Go2Model:
|
||||||
'Minv':self.Minv_,
|
'Minv':self.Minv_,
|
||||||
'nle':self.nle_,
|
'nle':self.nle_,
|
||||||
'g':self.g_,
|
'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):
|
def getGroundReactionForce(self, tau_est, body_acceleration=None):
|
||||||
|
|
|
||||||
|
|
@ -60,4 +60,30 @@ class KeyboardRemote(BaseRemote):
|
||||||
|
|
||||||
def flushStates(self):
|
def flushStates(self):
|
||||||
self.stand_up_down_seq_flag = False
|
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
|
||||||
|
|
||||||
|
|
@ -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
|
||||||
|
}
|
||||||
|
|
@ -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
|
||||||
|
}
|
||||||
|
|
@ -79,6 +79,45 @@
|
||||||
"source": [
|
"source": [
|
||||||
"Rotation.from_matrix(np.eye(3)).as_quat()"
|
"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": {
|
"metadata": {
|
||||||
|
|
|
||||||
|
|
@ -1,5 +1,12 @@
|
||||||
{
|
{
|
||||||
"cells": [
|
"cells": [
|
||||||
|
{
|
||||||
|
"cell_type": "markdown",
|
||||||
|
"metadata": {},
|
||||||
|
"source": [
|
||||||
|
"# Lowlevel Interface Test"
|
||||||
|
]
|
||||||
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 1,
|
"execution_count": 1,
|
||||||
|
|
@ -51,6 +58,69 @@
|
||||||
" robot.setCommands(q, dq, kp, kd, tau)\n",
|
" robot.setCommands(q, dq, kp, kd, tau)\n",
|
||||||
" time.sleep(0.01) "
|
" 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": {
|
"metadata": {
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue