我们安装V-REP之后，使用的是下面Git克隆的项目。

git clone https://github.com/deep-reinforcement-learning_book/Chapter16-Robot-Learning-in-Simulation.git

项目中直接组装好了一个机械臂。

我们先来分析下它的对象树

DefaultCamera:摄像机，用于捕捉场景的视图

Dummy:虚拟对象，作为参照或者用于定位其他对象

target:抓住任务中的目标物体。机器人需要抓取的物体

Vision_sensor:视觉传感器，用于捕捉场景图像，通常用于视觉反馈或识别目标

diningTable:场景中的桌子，通常作为抓取任务的基础

Sawyer:机器人手臂，执行抓取任务

Sawyer_jointX:机器人手臂的关节，用于控制各个链接之间的运动

Sawyer_tip:机器人的末端执行器，用于抓取物体

BaxterGripper:夹具部分，抓取物体的执行器

BaxterGripper_attachPoint:夹具的附加点，链接夹具和机器人手臂

BaxterGripper_attachProxSensor:接近传感器，用于检测夹具是否接近目标物体

BaxterGripper_leftJoint和BaxterGripper_rightJoint:夹具的左右关节，控制夹具的打开和关闭

BaxterGripper_leftPad和BaxterGripper_rightPad:夹具的抓取垫,实际接触目标物体的部分

在V-REP中设置好环境之后，我们需要用PyRep软件包写一个定义环境中动力学过程和奖励函数的控制脚本。我们仓库提供了定义环境的代码，我们来看下代码。

导入库和模块

from os.path import dirname, join, abspath
from pyrep import PyRep   #用与物理仿真环境交互的库
from pyrep.robots.arms.sawyer import Sawyer #机器人手臂和夹具的类
from pyrep.robots.end_effectors.baxter_gripper import BaxterGripper #夹具的类
from pyrep.objects.proximity_sensor import ProximitySensor #用于检测物体的传感器类
from pyrep.objects.vision_sensor import VisionSensor
from pyrep.objects.shape import Shape #用于场景中的物体和虚拟目标
from pyrep.objects.dummy import Dummy
from pyrep.const import JointType, JointMode #常量类，用于定义关节类型和控制模式
import numpy as np
import matplotlib.pyplot as plt
import math

定义有效位置范围

POS_MIN, POS_MAX = [0.1, -0.3, 1.], [0.45, 0.3, 1.]  # valid position range of target object 

创建GraspEnv类

class GraspEnv(object):
    ''' Sawyer robot grasping a cuboid '''

设置公共变量

self.headless = headless #设置可视化
self.reward_offset = 10.0  #成功抓取目标物体时的奖励值
self.reward_range = self.reward_offset#奖励范围，用于注册Gym环境
self.penalty_offset = 1.    #不良情况的惩罚值
self.fall_down_offset = 0.1 #判断目标物体是否掉落的距离阈值
self.metadata = []          #存储与Gym环境相关的元数据
self.control_mode = control_mode #设置控制模式

启动和设置场景

self.pr = PyRep()   #创建一个PyRep实例，用于与仿真环境交互
if control_mode == 'end_position':  #根据控制模式选择不同的场景文件
    SCENE_FILE = join(dirname(abspath(__file__)), './scenes/sawyer_reacher_rl_new_ik.ttt')
elif control_mode == 'joint_velocity':
    SCENE_FILE = join(dirname(abspath(__file__)), './scenes/sawyer_reacher_rl_new.ttt')
self.pr.launch(SCENE_FILE, headless=headless)
self.pr.start()

根据控制模式选择不同的场景文件，end_position 模式使用逆向运动学场景，而 joint_velocity 使用正向运动学场景。

获取机器人和夹具

self.agent = Sawyer() #为场景中Sawyer机器人手臂实例
self.gripper = BaxterGripper() #为场景中的Baxter Gripper夹具实例

设置传感器和物体

self.gripper_left_pad = Shape('BaxterGripper_leftPad')  #夹具的左垫，左侧接触面
self.proximity_sensor = ProximitySensor('BaxterGripper_attachProxSensor')#接近传感器
self.vision_sensor = VisionSensor('Vision_sensor') #视觉传感器
self.table  = Shape('diningTable') #桌子，在场景中用于碰撞检测的物体

控制模式设置

if control_mode == 'end_position': #根据控制模式启用或禁用控制循环。
    self.agent.set_control_loop_enabled(True) #根据控制模式定义动作空间的维度
    self.action_space = np.zeros(4)
elif control_mode == 'joint_velocity':
    self.agent.set_control_loop_enabled(False)
    self.action_space = np.zeros(7)
else:
    raise NotImplementedError

观察空间设置

self.observation_space = np.zeros(17)

包含7个关节的位置和速度以及目标的位置，总共17维。

锁定电机

self.agent.set_motor_locked_at_zero_velocity(True)

防止电机在初始化时移动。

获取目标和末端执行器

self.target = Shape('target') #目标物体
self.agent_ee_tip = self.agent.get_tip()#末端执行器
self.tip_target = Dummy('Sawyer_target')#目标点
self.tip_pos = self.agent_ee_tip.get_position()#获取末端执行器的当前位置

末端执行器（End Effector）是机器人或自动化系统中用于与环境进行交互的部件。它通常位于机器人手臂的末端，负责执行特定的任务，如抓取、搬运、切割、焊接等。

末端执行器的类型：夹具，吸盘，工具，手，喷嘴

设置初始姿态

if control_mode == 'end_position':  #设置末端执行器的初始位置和朝向
    initial_pos = [0.3, 0.1, 0.9]  #关节速度控制，设置各个关节的初始位置
    self.tip_target.set_position(initial_pos)
    self.tip_target.set_orientation([0, np.pi, np.pi/2], reset_dynamics=True)
elif control_mode == 'joint_velocity':
    self.initial_joint_positions = [0.001815199851989746, -1.4224984645843506, 0.704303503036499, 2.54307222366333, 2.972468852996826, -0.4989511966705322, 4.105560302734375]
    self.agent.set_joint_positions(self.initial_joint_positions)

记录初始位置

self.pr.step()   #更新仿真，进行一步仿真以更新状态
self.initial_tip_positions = self.agent_ee_tip.get_position()#获取末端执行器和目标物体的初始位置
self.initial_target_positions = self.target.get_position()
 

其他函数设置

_get_state:返回机器人的状态，包括关节位置，速度和目标位置

def _get_state(self):
    return np.array(self.agent.get_joint_positions()+
            self.agent.get_joint_velocities()+
            self.target.get_position())

_is_holding:检查夹具是否抓住物体

def _is_holding(self):
    pad_collide_object = self.gripper_left_pad.check_collision(self.target)#检查碰撞
    if pad_collide_object and self.proximity_sensor.is_detected(self.target)==True:
        return True  #检测目标
    else:
        return False

_move:根据给定的动作移动机器人末端执行器

函数_move可以在有效范围内通过反向运动学模式操控移动机械臂末端执行器，PuRep中可以谈股票在机械臂末端放置一个部件来实现以反向运动学控制末端执行器，具体做法是设置这个末端不见的位置和旋转角，如果调用pr.step()函数，你们在PyRep中机械臂关节的反向运动控制可以自动求解。由于单个较大步长的控制可能是不精确的，这里我们将整个动作产生的位移运动分解为一系列小步长运动，并采用一个有u自大迭代次数和最大容错值的反馈控制闭环来执行这些小步长动作。

def _move(self,action,bounding_offset=0.15,step_factor=0.2,max_itr=20,
    max_error=0.05,rotarion_norm=5.):
    pos=self.gripper.get_position()
    if pos[0]+action[0]>POS_MIN[0]-bounding_offset and pos[0]+action[0]<POS_MAX[0]+bounding_offset \
        and pos[1]+action[1]>POS_MIN[1]-bounding_offset and pos[1]+action[1]<POS_MAX[1]+2*bounding_offset \
        and pos[2]+action[2] > POS_MIN[2]-2*bounding_offset:  
        
        ori_z=-self.agent_ee_tip.get_orientation()[2] 
        target_pos = np.array(self.agent_ee_tip.get_position())+np.array(action[:3])
        diff=1 
        itr=0
        while np.sum(np.abs(diff))>max_error and itr<max_itr:
            itr+=1
            cur_pos = self.agent_ee_tip.get_position()
            diff=target_pos-cur_pos  #计算当前和目标位置之间的差
            pos = cur_pos+step_factor*diff   #根据当前差值和步长因子计算新位置
            self.tip_target.set_position(pos.tolist())
            self.pr.step()     #每次设置目标位置后，调用仿真步骤以更新状态
        ori_z+=rotation_norm*action[3]  
        self.tip_target.set_orientation([0, np.pi, ori_z])  
        self.pr.step()

    else:
        print("Potential Movement Out of the Bounding Box!")
        pass 

这段代码首先获取夹具当前的位置，

然后检查移动是否在有效范围内，如果新位置超出边界，则不执行移动。

获取当前末端执行器的朝向，特别是 Z 轴的旋转角度，并进行反转（因为在设置时方向可能存在差异）。

计算目标位置并进行移动，并通过一个循环逐步移动末端执行器到目标位置。计算当前位置与目标位置之间的差值 diff，并根据步长因子更新当前位置。

更新末端执行器的朝向，

边界检测失败时的处理。

reinit:重新初始化环境

def reset(self,random_target = False):
    self.shutdown()   #关闭原环境
    self.__init__(self.headless)#重新初始化环境，确保环境处于一个干净的状态

reset:重置环境，设置目标位置

def reset(self, random_target=False):
    '''
    在一个立方体内获取随机位置并设置目标位置。
    '''
    # 设置目标物体
    if random_target:  # 随机化
        pos = list(np.random.uniform(POS_MIN, POS_MAX))  # 从有效范围内均匀抽样
        self.target.set_position(pos)  # 随机位置
    else:  # 非随机化
        self.target.set_position(self.initial_target_positions)  # 固定位置
    self.target.set_orientation([0,0,0])  # 设置目标物体的朝向为零
    self.pr.step()  # 进行一步仿真

    # 设置末端位置以进行初始化
    if self.control_mode == 'end_position':  # 关节模式为逆运动学
        self.agent.set_control_loop_enabled(True)  # 启用逆运动学模式
        self.tip_target.set_position(self.initial_tip_positions)  # 由于在逆运动学模式下不能直接设置关节位置，因此设置末端目标位置
        self.pr.step()  # 进行一步仿真
        # 防止卡住的情况。由于使用逆运动学进行移动，卡住可能导致逆运动学无法求解，因此在目标位置未达到时采取一些随机动作。
        itr = 0
        max_itr = 10
        while np.sum(np.abs(np.array(self.agent_ee_tip.get_position() - np.array(self.initial_tip_positions)))) > 0.1 and itr < max_itr:
            itr += 1
            self.step(np.random.uniform(-0.2, 0.2, 4))  # 采取随机动作以防止卡住
            self.pr.step()  # 进行一步仿真

    elif self.control_mode == 'joint_velocity':  # 关节模式为前向运动学
        self.agent.set_joint_positions(self.initial_joint_positions)  # 设置初始关节位置
        self.pr.step()  # 进行一步仿真

    # 设置可碰撞，以便进行碰撞检测
    self.gripper_left_pad.set_collidable(True)  # 设置夹具的左垫为可碰撞，以便检查碰撞
    self.target.set_collidable(True)  # 设置目标物体为可碰撞
    # 如果夹具没有完全打开，则打开夹具
    if np.sum(self.gripper.get_open_amount()) < 1.5:
        self.gripper.actuate(1, velocity=0.5)  # 以一定速度打开夹具
        self.pr.step()  # 进行一步仿真

    return self._get_state()  # 返回当前环境状态

step:根据动作更新环境状态，计算奖励和完成状态

def step(self, action):
    '''
    根据给定的动作移动机器人手臂。
    如果控制模式为 'joint_velocity'，则动作是 7 维的关节速度值 + 1 维夹具的旋转值；
    如果控制模式为 'end_position'，则动作是 3 维的末端（手臂末端）位置值 + 1 维夹具的旋转值。
    '''
    # 初始化
    done = False  # 表示回合是否结束
    reward = 0  # 奖励初始化
    hold_flag = False  # 是否抓住物体的标志

    if self.control_mode == 'end_position':
        if action is None or action.shape[0] != 4:  # 检查动作是否有效
            print('没有动作或动作维度错误！')
            action = list(np.random.uniform(-0.1, 0.1, 4))  # 生成随机动作
        self._move(action)  # 根据动作移动末端执行器

    elif self.control_mode == 'joint_velocity':
        if action is None or action.shape[0] != 7:  # 检查动作是否有效
            print('没有动作或动作维度错误！')
            action = list(np.random.uniform(-0.1, 0.1, 7))  # 生成随机动作
        self.agent.set_joint_target_velocities(action)  # 在手臂上执行动作
        self.pr.step()  # 进行一步仿真

    else:
        raise NotImplementedError  # 如果控制模式不支持，抛出异常

    ax, ay, az = self.gripper.get_position()  # 获取夹具的当前位置
    if math.isnan(ax):  # 捕捉夹具位置为 NaN 的情况
        print('夹具位置为 NaN。')
        self.reinit()  # 重新初始化环境
        done = True  # 标记为结束

    tx, ty, tz = self.target.get_position()  # 获取目标物体的位置
    offset = 0.08  # 增强奖励：目标位置在目标物体上方的偏移量
    sqr_distance = (ax - tx) ** 2 + (ay - ty) ** 2 + (az - (tz + offset)) ** 2  # 夹具与目标物体之间的平方距离

    # ''' 仅用于基于视觉的控制，时间消耗大！ '''
    # current_vision = self.vision_sensor.capture_rgb()  # 使用视觉传感器捕捉视图的截图
    # plt.imshow(current_vision)  # 显示捕捉到的图像
    # plt.savefig('./img/vision.png')  # 保存图像

    # 如果夹具与物体足够接近，并且接近传感器检测到目标，则关闭夹具
    if sqr_distance < 0.1 and self.proximity_sensor.is_detected(self.target) == True:
        # 确保在抓取之前夹具是打开的
        self.gripper.actuate(1, velocity=0.5)  # 打开夹具
        self.pr.step()  # 进行一步仿真
        self.gripper.actuate(0, velocity=0.5)  # 关闭夹具，0 表示关闭，1 表示打开；速度 0.5 确保夹具在一帧内关闭
        self.pr.step()  # 进行一步仿真

        if self._is_holding():  # 检查是否抓住物体
            reward += self.reward_offset  # 抓取物体时的额外奖励
            done = True  # 标记为结束
            hold_flag = True  # 设置抓住物体的标志
        else:
            self.gripper.actuate(1, velocity=0.5)  # 如果没有抓住物体，确保夹具是打开的
            self.pr.step()  # 进行一步仿真
    elif np.sum(self.gripper.get_open_amount()) < 1.5:  # 如果夹具关闭（未完全打开），则打开夹具
        self.gripper.actuate(1, velocity=0.5)  # 打开夹具
        self.pr.step()  # 进行一步仿真
    else:
        pass  # 如果夹具状态正常，什么都不做

    # 基础奖励是与目标的负距离
    reward -= np.sqrt(sqr_distance)  # 根据夹具与目标的距离调整奖励

    # 检查物体是否掉落
    if tz < self.initial_target_positions[2] - self.fall_down_offset:  
        done = True  # 标记为结束
        reward = -self.reward_offset  # 物体掉落的惩罚奖励

    # 增强奖励：如果夹具的朝向与目标物体垂直，则抓取姿势更好
    desired_orientation = np.concatenate(([np.pi, 0], [self.target.get_orientation()[2]]))  # 夹具在 z 轴上垂直于目标并朝下
    rotation_penalty = -np.sum(np.abs(np.array(self.agent_ee_tip.get_orientation()) - desired_orientation))  # 计算朝向惩罚
    rotation_norm = 0.02  # 旋转惩罚的归一化因子
    reward += rotation_norm * rotation_penalty  # 将旋转惩罚添加到奖励中

    # 碰撞惩罚
    if self.gripper_left_pad.check_collision(self.table):  # 检查夹具是否与桌子碰撞
        reward -= self.penalty_offset  # 如果发生碰撞，减少奖励
        # print('惩罚与桌子的碰撞。')

    if math.isnan(reward):  # 捕捉数值问题的情况
        reward = 0.  # 如果奖励为 NaN，重置为 0

    return self._get_state(), reward, done, {'finished': hold_flag}  # 返回当前状态、奖励、是否结束和抓取标志

shutdown:关闭仿真

def shutdown(self):
        self.pr.stop()
        self.pr.shutdown()

主程序

if __name__ == '__main__':
#设置控制模式
    CONTROL_MODE='joint_velocity'  # 'end_position' or 'joint_velocity'
    env=GraspEnv(headless=False, control_mode=CONTROL_MODE) #初始化抓取环境
    for eps in range(30):
        env.reset() #每个回合的开始，重置环境
        for step in range(30):
            if CONTROL_MODE=='end_position': #根据控制模式生成动作
                action=np.random.uniform(-0.2,0.2,4)
            elif CONTROL_MODE=='joint_velocity':
                action=np.random.uniform(-2.,2.,7)
            else:
                raise NotImplementedError
            try: 
                env.step(action)   #执行生成的动作并更新环境       
            except KeyboardInterrupt:
                print('Shut Down!')
                env.shutdown()

    env.shutdown()