YOLO_World-SAM-GraspNet-Cuda 12.x-python-3.10

1.环境构建1.1 Cuda切换参考博客https://www.cnblogs.com/john-mu-wanfeng/p/19071862进行cuda安装切换cuda官网https://developer.nvidia.com/cuda-toolkit-archivecudnn官网https://developer.nvidia.com/rdp/cudnn-archive1.2 graspnet环境配置下载YOLO_World-SAM-GraspNet项目百度网盘链接: https://pan.baidu.com/s/1PoDS7odFY6rfxeVfQSn4SQ 提取码: yolo最近链接实效了贴另一个佬提供的链接https://pan.baidu.com/s/1NNeS5C_9irYVFHOGCWR0ZQ rri4#1.创建环境 conda create -n mujoco_graspnet python3.10 conda activate mujoco_graspnet #2.进入项目的graspnet-baseline文件 cd graspnet-baseline #修改requirements.txt sudo gedit requirements.txt #这里要指定numpy的安装版本为1.23.0,和scipy版本匹配。删除torch最后如下所示 #tensorboard #numpy1.23.0 #scipy #open3d0.8 #Pillow #tqdm #安装graspnet-baseline环境所依赖 pip install -r requirements.txt #4.安装torch等所需的环境包这里默认直接安装torch就好选择cuda版本12.x pip install torch torchvision torchaudio #需指定cuda版本 pip install spatialmath-python1.1.14 pip install roboticstoolbox-python1.1.1 pip install modern-robotics1.1.1 pip install mujoco3.3.1 #5.编译并安装PointNet自定义算子 cd pointnet2 python setup.py install cd ../ #6.编译并安装基于PyTorch CUDA实现的k-最近邻k-NN算子 cd knn python setup.py install cd ../ #7.安装graspnetAPI工具包 cd graspnetAPI #修改setup.py将sklearn替换为scikit-learn pip install . cd ../ 8.安装KDL运动学解算包 conda install python-orocos-kdl 9.补充额外依赖 pip install ultralytics pip install lxml pip install githttps://github.com/openai/CLIP.git1.3 测试运行直接执行python main_yoloWorld_sam.py2.工具坐标系切换由于我在使用该项目时需要控制rm eco65臂因此仿真环境内需要切换到eco65臂的urdf和scene.xml来代替原本的ur5机械臂。修改manipulator_grasp/env中的env.py文件。class RMEco65Env: def __init__(self): pass #同ur5 def reset(self): # 初始化 MuJoCo 模型和数据 filename os.path.join(os.path.dirname(os.path.dirname(__file__)), assets, rm_eco65_scene, scene.xml) # 创建机械臂实例并设置其基座位置 self.robot EM_ECO65() # 工具偏移 robot_tool sm.SE3.RPY(np.pi/2, -np.pi/2, np.pi)*sm.SE3.Trans(0.142, 0.0, 0.0) #其它同ur5 def close(self): pass #同ur5 def step(self, actionNone): pass #同ur5 def render(self): pass #同ur5 if __name__ __main__: env RMEco65Env() env.reset() for i in range(1000000): env.step() imgs env.render() env.close()由于末端坐标系不同工具坐标系增加了额外的旋转和平移robot_tool sm.SE3.RPY(np.pi/2, -np.pi/2, np.pi)*sm.SE3.Trans(0.142, 0.0, 0.0)机器人xml场景mujoco modelrm_eco65_with_gripper compiler angleradian/ default include file./rm_eco65_classes.xml/ /default asset !-- 机械臂网格 -- mesh namebaselink content_typemodel/stl file./mesh/rm_eco65_arm/baselink.STL/ mesh nameLink1 content_typemodel/stl file./mesh/rm_eco65_arm/Link1.STL/ mesh nameLink2 content_typemodel/stl file./mesh/rm_eco65_arm/Link2.STL/ mesh nameLink3 content_typemodel/stl file./mesh/rm_eco65_arm/Link3.STL/ mesh nameLink4 content_typemodel/stl file./mesh/rm_eco65_arm/Link4.STL/ mesh nameLink5 content_typemodel/stl file./mesh/rm_eco65_arm/Link5.STL/ mesh nameLink6 content_typemodel/stl file./mesh/rm_eco65_arm/Link6.STL/ !-- 夹爪网格 -- mesh namebase_mount file./assets/base_mount.stl scale0.001 0.001 0.001/ mesh namebase file./assets/base.stl scale0.001 0.001 0.001/ mesh namedriver file./assets/driver.stl scale0.001 0.001 0.001/ mesh namecoupler file./assets/coupler.stl scale0.001 0.001 0.001/ mesh namefollower file./assets/follower.stl scale0.001 0.001 0.001/ mesh namepad file./assets/pad.stl scale0.001 0.001 0.001/ mesh namesilicone_pad file./assets/silicone_pad.stl scale0.001 0.001 0.001/ mesh namespring_link file./assets/spring_link.stl scale0.001 0.001 0.001/ !-- 材质定义 -- material namemetal rgba0.58 0.58 0.58 1/ material namesilicone rgba0.1882 0.1882 0.1882 1/ material namegray rgba0.4627 0.4627 0.4627 1/ material nameblack rgba0.149 0.149 0.149 1/ /asset default default class2f85 mesh scale0.001 0.001 0.001/ general biastypeaffine/ joint axis1 0 0/ default classdriver joint range0 0.8 armature0.005 damping0.1 solimplimit0.95 0.99 0.001 solreflimit0.005 1/ /default default classfollower joint range-0.872664 0.872664 armature0.001 pos0 -0.018 0.0065 solimplimit0.95 0.99 0.001 solreflimit0.005 1/ /default default classspring_link joint range-0.29670597283 0.8 armature0.001 stiffness0.05 springref2.62 damping0.00125/ /default default classcoupler joint range-1.57 0 armature0.001 solimplimit0.95 0.99 0.001 solreflimit0.005 1/ /default default classvisual geom typemesh contype0 conaffinity0 group2/ /default default classcollision geom typemesh group3/ default classpad_box1 geom mass0 typebox pos0 -0.0026 0.028125 size0.011 0.004 0.009375 friction0.7 solimp0.95 0.99 0.001 solref0.004 1 priority1 rgba0.55 0.55 0.55 1/ /default default classpad_box2 geom mass0 typebox pos0 -0.0026 0.009375 size0.011 0.004 0.009375 friction0.6 solimp0.95 0.99 0.001 solref0.004 1 priority1 rgba0.45 0.45 0.45 1/ /default /default /default /default worldbody !-- 机械臂主体 -- body nameeco65_base pos0.80 0.6 0.745 quat0 0 0 1 geom typemesh rgba1 1 0.9451 1 meshbaselink/ body nameLink1 pos0 0 0.1625 inertial pos-0.022858 -2.0784e-06 -0.013658 quat0.422359 0.558164 0.572915 0.426419 mass0.74247 diaginertia0.00184837 0.00159481 0.000747401/ joint namejoint1 classsize3_limited pos0 0 0 axis0 0 1 armature0.0005 range-3.1067 3.1067/ geom typemesh rgba1 1 0.9451 1 meshLink1/ body nameLink2 pos-0.086 0 0 quat0.707105 0.707108 0 0 inertial pos-1.6049e-06 0.10336 0.067332 quat0.49284 0.507114 -0.50706 0.492782 mass1.3502 diaginertia0.0129426 0.012821 0.00111406/ joint namejoint2 classsize3_limited pos0 0 0 axis0 0 -1 armature0.0005 range-3.1067 2.3562/ geom typemesh rgba1 1 0.9451 1 meshLink2/ body nameLink3 pos0 0.26 0 quat-2.59734e-06 0.707105 0.707108 -2.59735e-06 inertial pos0.10374 7.0495e-07 0.018599 quat0.519814 0.479367 0.479376 0.519807 mass1.0422 diaginertia0.0094046 0.00930383 0.000785521/ joint namejoint3 classsize3_limited pos0 0 0 axis0 0 1 armature0.0005 range-2.7925 2.5307/ geom typemesh rgba1 1 0.9451 1 meshLink3/ body nameLink4 pos0.24 0 -0.05888 quat0.707105 0 0 0.707108 inertial pos-0.0069819 -0.016328 0.0067416 quat0.446544 0.894762 -6.23183e-05 5.65167e-06 mass0.27383 diaginertia0.00024751 0.000214431 0.000154999/ joint namejoint4 classsize3_limited pos0 0 0 axis0 0 1 armature0.0005 range-3.1067 3.1067/ geom typemesh rgba1 1 0.9451 1 meshLink4/ body nameLink5 pos0.00040486 -0.10983 0 quat0.707105 0.707108 0 0 inertial pos-0.00040425 0.0066328 -0.012475 quat0.446544 0.894762 -6.23183e-05 5.65167e-06 mass0.27794 diaginertia0.00024751 0.000214431 0.000154999/ joint namejoint5 classsize3_limited pos0 0 0 axis0 0 1 armature0.0005 range-3.1067 3.1067/ geom typemesh rgba0.29804 0.28627 0.28235 1 meshLink5/ body nameLink6 pos0 0.0795 0 quat0.707105 -0.707108 0 0 inertial pos0.00095587 -0.0013555 -0.010297 quat0.594994 0.390437 -0.592979 0.37672 mass0.18 diaginertia0.000132134 9.90863e-05 9.90863e-05/ joint namejoint6 classsize1 pos0 0 0 axis0 0 1 armature0.01 range-6.2832 6.2832 damping0.3/ geom typemesh rgba1 1 0.9451 1 meshLink6/ site nametcp pos0 0 0 typebox size0.01 0.01 0.01 rgba0 1 0 0.5 group2 / !-- 在这里连接夹爪到机械臂末端 -- body name2f85_base pos0 0 0 quat0 0 0 1 childclass2f85 geom classvisual meshbase_mount materialblack/ geom classcollision meshbase_mount/ body namebase pos0 0 0.0038 quat1 0 0 -1 inertial mass0.777441 pos0 -2.70394e-05 0.0354675 quat1 -0.00152849 0 0 diaginertia0.000260285 0.000225381 0.000152708/ geom classvisual meshbase materialblack/ geom classcollision meshbase/ site namepinch pos0 0 0.145 typesphere group5 rgba0.9 0.9 0.9 1 size0.005/ !-- 右侧4杆机构 -- body nameright_driver pos0 0.0306011 0.054904 inertial mass0.00899563 pos2.96931e-12 0.0177547 0.00107314 quat0.681301 0.732003 0 0 diaginertia1.72352e-06 1.60906e-06 3.22006e-07/ joint nameright_driver_joint classdriver/ geom classvisual meshdriver materialgray/ geom classcollision meshdriver/ body nameright_coupler pos0 0.0315 -0.0041 inertial mass0.0140974 pos0 0.00301209 0.0232175 quat0.705636 -0.0455904 0.0455904 0.705636 diaginertia4.16206e-06 3.52216e-06 8.88131e-07/ joint nameright_coupler_joint classcoupler/ geom classvisual meshcoupler materialblack/ geom classcollision meshcoupler/ /body /body body nameright_spring_link pos0 0.0132 0.0609 inertial mass0.0221642 pos-8.65005e-09 0.0181624 0.0212658 quat0.663403 -0.244737 0.244737 0.663403 diaginertia8.96853e-06 6.71733e-06 2.63931e-06/ joint nameright_spring_link_joint classspring_link/ geom classvisual meshspring_link materialblack/ geom classcollision meshspring_link/ body nameright_follower pos0 0.055 0.0375 inertial mass0.0125222 pos0 -0.011046 0.0124786 quat1 0.1664 0 0 diaginertia2.67415e-06 2.4559e-06 6.02031e-07/ joint nameright_follower_joint classfollower/ geom classvisual meshfollower materialblack/ geom classcollision meshfollower/ body nameright_pad pos0 -0.0189 0.01352 geom classpad_box1 nameright_pad1/ geom classpad_box2 nameright_pad2/ inertial mass0.0035 pos0 -0.0025 0.0185 quat0.707107 0 0 0.707107 diaginertia4.73958e-07 3.64583e-07 1.23958e-07/ geom classvisual meshpad/ body nameright_silicone_pad geom classvisual meshsilicone_pad materialblack/ /body /body /body /body !-- 左侧4杆机构 -- body nameleft_driver pos0 -0.0306011 0.054904 quat0 0 0 1 inertial mass0.00899563 pos0 0.0177547 0.00107314 quat0.681301 0.732003 0 0 diaginertia1.72352e-06 1.60906e-06 3.22006e-07/ joint nameleft_driver_joint classdriver/ geom classvisual meshdriver materialgray/ geom classcollision meshdriver/ body nameleft_coupler pos0 0.0315 -0.0041 inertial mass0.0140974 pos0 0.00301209 0.0232175 quat0.705636 -0.0455904 0.0455904 0.705636 diaginertia4.16206e-06 3.52216e-06 8.88131e-07/ joint nameleft_coupler_joint classcoupler/ geom classvisual meshcoupler materialblack/ geom classcollision meshcoupler/ /body /body body nameleft_spring_link pos0 -0.0132 0.0609 quat0 0 0 1 inertial mass0.0221642 pos-8.65005e-09 0.0181624 0.0212658 quat0.663403 -0.244737 0.244737 0.663403 diaginertia8.96853e-06 6.71733e-06 2.63931e-06/ joint nameleft_spring_link_joint classspring_link/ geom classvisual meshspring_link materialblack/ geom classcollision meshspring_link/ body nameleft_follower pos0 0.055 0.0375 inertial mass0.0125222 pos0 -0.011046 0.0124786 quat1 0.1664 0 0 diaginertia2.67415e-06 2.4559e-06 6.02031e-07/ joint nameleft_follower_joint classfollower/ geom classvisual meshfollower materialblack/ geom classcollision meshfollower/ body nameleft_pad pos0 -0.0189 0.01352 geom classpad_box1 nameleft_pad1/ geom classpad_box2 nameleft_pad2/ inertial mass0.0035 pos0 -0.0025 0.0185 quat1 0 0 1 diaginertia4.73958e-07 3.64583e-07 1.23958e-07/ geom classvisual meshpad/ body nameleft_silicone_pad geom classvisual meshsilicone_pad materialblack/ /body /body /body /body /body /body /body /body /body /body /body /body /body /worldbody contact exclude body1base body2left_driver/ exclude body1base body2right_driver/ exclude body1base body2left_spring_link/ exclude body1base body2right_spring_link/ exclude body1right_coupler body2right_follower/ exclude body1left_coupler body2left_follower/ /contact tendon fixed namesplit joint jointright_driver_joint coef0.5/ joint jointleft_driver_joint coef0.5/ /fixed /tendon equality connect anchor0 0 0 body1right_follower body2right_coupler solimp0.95 0.99 0.001 solref0.005 1/ connect anchor0 0 0 body1left_follower body2left_coupler solimp0.95 0.99 0.001 solref0.005 1/ joint joint1right_driver_joint joint2left_driver_joint polycoef0 1 0 0 0 solimp0.95 0.99 0.001 solref0.005 1/ /equality actuator !-- 机械臂关节执行器 -- general classsize3 namejoint1_act jointjoint1/ general classsize3 namejoint2_act jointjoint2/ general classsize3 namejoint3_act jointjoint3/ general classsize1 namejoint4_act jointjoint4/ general classsize1 namejoint5_act jointjoint5/ general classsize1 namejoint6_act jointjoint6/ !-- 夹爪执行器 -- general class2f85 namefingers_actuator tendonsplit forcerange-5 5 ctrlrange0 255 gainprm0.3137255 0 0 biasprm0 -100 -10/ /actuator /mujococlass,scene可根据需要模仿ur5自行调整。3.逆运动学ur5采用其特有的解算方法对于其它的机械臂并不通用为了仿真时准确解算出目标关节角度需要引入通用的逆运动学解算方法这里使用kdl算法代替。创建EM_ECO65类类似UR5继承Robot修改ikfk和move_cartesian函数import os import sys # 自动补充项目根目录和manipulator_grasp到sys.path兼容直接运行和包方式 ROOT_DIR os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) sys.path.append(ROOT_DIR) sys.path.append(os.path.join(ROOT_DIR, manipulator_grasp)) from typing import List import numpy as np import roboticstoolbox as rtb from spatialmath import SE3 import modern_robotics as mr from arm.geometry import Geometry3D, Capsule from arm.utils import MathUtils from .robot_config import RobotConfig from .robot import Robot from kdl_parser.urdf_parser_py.urdf import URDF from kdl_parser.urdf import treeFromFile, treeFromUrdfModel import PyKDL class EM_ECO65(Robot): def __init__(self) - None: super().__init__() # RM_ECO65 机械臂参数 success, tree self.load_urdf_to_kdl(eco65 urdf路径) base_link baselink tip_link Link6 chain self.create_chain_from_tree(tree, base_link, tip_link) self.fk_solver, self.ik_vel_solver, self.ik_pos_solver, self.chain self.setup_kinematics(chain) self.q0 [0.2, 0.2, -2.5, 0.4, 0.5, 0.6] def load_urdf_to_kdl(self, urdf_file_path: str): 从URDF文件加载KDL树结构 参数: urdf_file_path: URDF文件的路径 返回: success: 加载是否成功 tree: KDL树对象 if not os.path.exists(urdf_file_path): print(furdf not exists - {urdf_file_path}) return False, None try: success, tree treeFromFile(urdf_file_path) if success: print(fload sucess: {urdf_file_path}) print(fchain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints) return success, tree except Exception as e: print(fmethods1 failed: {e}) # 使用二进制模式读取文件并移除编码声明 try: with open(urdf_file_path, rb) as f: # 读取二进制数据 xml_content f.read() # 尝试解码为字符串 try: xml_str xml_content.decode(utf-8) except UnicodeDecodeError: # 尝试其他编码 xml_str xml_content.decode(latin-1) # 移除XML编码声明 if xml_str.startswith(?xml): xml_str xml_str[xml_str.find(?) 2:] # 从字符串创建URDF模型 robot_model URDF.from_xml_string(xml_str) # 从URDF模型创建KDL树 success, tree treeFromUrdfModel(robot_model) if success: print(fmethod2 sucess: {urdf_file_path}) print(fchain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints) return success, tree else: print(method2 failed) except Exception as e: print(fmethod2 failed: {e}) try: robot_model URDF.from_xml_file(urdf_file_path) success, tree treeFromUrdfModel(robot_model) if success: print(fmethod3 sucess: {urdf_file_path}) print(fchain has {tree.getNrOfSegments()} segs and {tree.getNrOfJoints()} joints) return success, tree else: print(method3 failed) except Exception as e: print(fmethod3 failed: {e}) print(fcan not load: {urdf_file_path}) return False, None def create_chain_from_tree(self, tree, base_link, tip_link): 从KDL树创建运动链 参数: tree: KDL树对象 base_link: 基链接名称 tip_link: 末端执行器链接名称 返回: chain: KDL链对象 try: chain tree.getChain(base_link, tip_link) print(fcreate chain from {base_link} to {tip_link}) print(fchain has {chain.getNrOfSegments()} segs and {chain.getNrOfJoints()} joints) return chain except Exception as e: print(fcreate chain failed: {e}) return None def setup_kinematics(self, chain): 设置正逆运动学求解器 参数: chain: KDL链对象 返回: fk_solver: 正向运动学求解器 ik_vel_solver: 逆速度运动学求解器 ik_pos_solver: 逆位置运动学求解器 chain: 运动链对象 (添加此返回值以便在其他函数中使用) fk_solver PyKDL.ChainFkSolverPos_recursive(chain) ik_vel_solver PyKDL.ChainIkSolverVel_pinv(chain) max_iterations 5000 eps 3*1e-2 ik_pos_solver PyKDL.ChainIkSolverPos_NR( chain, fk_solver, ik_vel_solver, max_iterations, eps ) return fk_solver, ik_vel_solver, ik_pos_solver, chain def perform_forward_kinematics(self, fk_solver, joint_positions): 执行正向运动学计算 参数: fk_solver: 正向运动学求解器 joint_positions: 关节位置列表 返回: frame: KDL Frame对象表示末端执行器的位姿 q PyKDL.JntArray(len(joint_positions)) for i in range(len(joint_positions)): q[i] joint_positions[i] frame PyKDL.Frame() status fk_solver.JntToCart(q, frame) if status 0: position [frame.p.x(), frame.p.y(), frame.p.z()] rotation frame.M.GetQuaternion() # print(ffk sucess:) # print(fpos: {position}) # print(fquat: {rotation}) return position, rotation, frame else: # print(fk failed) return None def fkine(self, q) - SE3: 使用KDL正向运动学求解器输入关节角q输出SE3位姿 if not hasattr(self, fk_solver): raise RuntimeError(KDL求解器未初始化请先调用setup_kdl_kinematics) position, quat, frame self.perform_forward_kinematics(self.fk_solver, q) if position is None: return SE3() # KDL四元数为(x, y, z, w)spatialmath为(w, x, y, z) from spatialmath import UnitQuaternion rot UnitQuaternion([quat[3], quat[0], quat[1], quat[2]]).R se3_pose SE3.Rt(rot, position) return se3_pose def perform_inverse_kinematics(self, ik_pos_solver, chain, target_frame, initial_jointsNone): 执行逆运动学计算 num_joints chain.getNrOfJoints() if initial_joints is None: q_init PyKDL.JntArray(num_joints) else: q_init PyKDL.JntArray(num_joints) joints list(initial_joints)[:num_joints] if len(joints) num_joints: joints [0.0] * (num_joints - len(joints)) for i in range(num_joints): q_init[i] joints[i] q_out PyKDL.JntArray(num_joints) status ik_pos_solver.CartToJnt(q_init, target_frame, q_out) if status 0: joint_positions [q_out[i] for i in range(num_joints)] # 角度归一化到[-pi, pi] joint_positions [((x np.pi) % (2 * np.pi)) - np.pi for x in joint_positions] # print(fik sucess:) # print(fJoints: {joint_positions}) return joint_positions else: print(ik failed) return None def ikine(self, Tep: SE3) - np.ndarray: 使用KDL逆运动学求解器输入为SE3输出为关节角度数组与原格式一致 补偿机械臂末端和夹爪末端的差异 # print(##################,self._base) # T06 self._base.inv() * Tep# * self._tool.inv() T06 Tep * self._tool.inv() # T06 Tep * self._tool.inv() pos T06.t rot T06.R # PyKDL.Rotation() 可直接接受9个元素 frame PyKDL.Frame( PyKDL.Rotation( rot[0,0], rot[0,1], rot[0,2], rot[1,0], rot[1,1], rot[1,2], rot[2,0], rot[2,1], rot[2,2] ), PyKDL.Vector(pos[0], pos[1], pos[2]) ) if not hasattr(self, ik_pos_solver): raise RuntimeError(KDL求解器未初始化请先调用setup_kdl_kinematics) joints self.perform_inverse_kinematics(self.ik_pos_solver, self.chain, frame, self.q0) if joints is None: return np.array([]) return np.array(joints) def move_cartesian(self, T: SE3): 移动机器人到指定的笛卡尔空间位姿。 参数: T: 目标位姿表示为SE3对象 异常: 如果逆运动学求解失败将抛出断言异常。 q self.ikine(T) if len(q) 0: import traceback print([IK FAILED] move_cartesian: SE3, T) # print(self.q0) traceback.print_stack() assert len(q), inverse kinematics failure self.q0 q[:] ##剩余类函数不变4.main函数修改根据KDL可以根据每一个移动步骤的目标位置姿态逆解出角度qTwo_in_base T_wb.inv() * T_wo#T_wb在world下的旋转矩阵 new_t Two_in_base.t.copy() new_t[2] 0.2 T2_base sm.SE3.Rt(Two_in_base.R, new_t) # 用平移后的T2_base_offset做逆解 self.q2 robot.ikine(T2_base) if isinstance(self.q2, tuple): self.q2 self.q2[0] time2 2 parameter2 JointParameter(self.q1, self.q2) velocity_parameter2 QuinticVelocityParameter(time2) trajectory_parameter2 TrajectoryParameter(parameter2, velocity_parameter2) planner2 TrajectoryPlanner(trajectory_parameter2) # 执行planner_array [planner2] time_array [0.0, time2] planner_array [planner2] total_time np.sum(time_array) time_step_num round(total_time / 0.002) 1 times np.linspace(0.0, total_time, time_step_num) time_cumsum np.cumsum(time_array) for timei in times: for j in range(len(time_cumsum)): if timei 0.0: break if timei time_cumsum[j]: planner_interpolate planner_array[j - 1].interpolate(timei - time_cumsum[j - 1]) if isinstance(planner_interpolate, np.ndarray): joint planner_interpolate robot.move_joint(joint) else: robot.move_cartesian(planner_interpolate) joint robot.get_joint() action[:6] joint env.step(action) break