开始这两章之前,最好先打下一点有关机械臂的基础
文章目录
Unit 7: Training a Fetch Robot. Part 1
| SUMMARY |
|---|
Estimated time to completion: 2 hours
In this unit, you are going to have a step-by-step look at how to build the Robot Environment for training a Fetch robot.
| END OF SUMMARY |
|---|
So, at this point, you should already have learned how the basic structure that you need to create works when you want to train a robot using OpenAI with ROS and Gazebo. You have already seen complete examples of this structure, both in the CartPole environment and with a moving Cube. But, that’s not all!
In this final part of the course, we are going to create all the environments and files that are necessary in order to train a manipulator robot: the Fetch robot. And this, as you will see throughout the following units, will complicate everything just a little bit. Fortunately, the environments structure that you’ve learned during this course will help us make things clearer and easier to understand. So… let’s go!
Robot Environment
We will start by creating our Robot Environment. This Robot Environment, as you may already know, will be in charge of providing an interface to communicate with the Fetch robot. This means that it will contain all the basic functions to control the Fetch robot; for instance, moving the manipulator arm.
We will start, as we did with the Cube Environment, with the basic robot environment template.
| template_my_robot_env.py |
|---|
from openai_ros import robot_gazebo_env
class MyRobotEnv(robot_gazebo_env.RobotGazeboEnv):
"""Superclass for all Robot environments.
"""
def __init__(self):
"""Initializes a new Robot environment.
"""
# Variables that we give through the constructor.
# Internal Vars
self.controllers_list = ['my_robot_controller1','my_robot_controller2', ..., 'my_robot_controllerX']
self.robot_name_space = "my_robot_namespace"
reset_controls_bool = True or False
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(MyRobotEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=reset_controls_bool)
# Methods needed by the Gazebo Environment
# ----------------------------
def _check_all_systems_ready(self):
"""
Checks that all the sensors, publishers and other simulation systems are
operational.
"""
# TODO
return True
# Methods that the Task Environment will need to define here as virtual
# because they will be used in RobotGazeboEnv GrandParentClass and defined in the
# TrainingEnvironment.
# ----------------------------
def _set_init_pose(self):
"""Sets the Robot in its init pose
"""
raise NotImplementedError()
def _init_env_variables(self):
"""Inits variables needed to be initialised each time we reset at the start
of an episode.
"""
raise NotImplementedError()
def _compute_reward(self, observations, done):
"""Calculates the reward to give based on the observations given.
"""
raise NotImplementedError()
def _set_action(self, action):
"""Applies the given action to the simulation.
"""
raise NotImplementedError()
def _get_obs(self):
raise NotImplementedError()
def _is_done(self, observations):
"""Checks if episode done based on observations given.
"""
raise NotImplementedError()
# Methods that the Robot Environment will need.
# ----------------------------
| template_my_robot_env.py |
|---|
| Exercise 7.1 |
|---|
a) Create a new package named my_fetch_train. Inside this package, create a new folder named scripts, where you will place all the files related to the Fetch robot.
b) Inside the scripts folder, create a new file named fetch_env_v2.py, and paste the above template inside. You can also rename the environment to FetchEnv. You will then do all the necessary changes to adapt it to the Fetch robot.
| End of Exercise 7.1 |
|---|
Initialization of the class
Let’s go by parts. First of all, we’ll need to fill the initialization of the class. Here you can see an example of how it could be:
def __init__(self):
"""Initializes a new Fetch environment.
"""
# We Start all the ROS related Subscribers and publishers
JOINT_STATES_SUBSCRIBER = '/joint_states'
self.joint_states_sub = rospy.Subscriber(JOINT_STATES_SUBSCRIBER, JointState, self.joints_callback)
self.joints = JointState()
self.ee_traj_client = rospy.ServiceProxy('/ee_traj_srv', EeTraj)
self.joint_traj_client = rospy.ServiceProxy('/joint_traj_srv', JointTraj)
self.ee_pose_client = rospy.ServiceProxy('/ee_pose_srv', EePose)
self.ee_rpy_client = rospy.ServiceProxy('/ee_rpy_srv', EeRpy)
# Variables that we give through the constructor.
self.controllers_list = []
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env_goal.RobotGazeboEnv
super(FetchEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False)
The first thing we are doing, as you can see, is creating a subscriber for the /joint_states topic, and also creating a variable of the JointState() type, where we will store the values of the joints of the robot.
JOINT_STATES_SUBSCRIBER = '/joint_states'
self.joint_states_sub = rospy.Subscriber(JOINT_STATES_SUBSCRIBER, JointState, self.joints_callback)
self.joints = JointState()
Next, we are defining a series of Service Clients, as you can see below.
self.ee_traj_client = rospy.ServiceProxy('/ee_traj_srv', EeTraj)
self.joint_traj_client = rospy.ServiceProxy('/joint_traj_srv', JointTraj)
self.ee_pose_client = rospy.ServiceProxy('/ee_pose_srv', EePose)
self.ee_rpy_client = rospy.ServiceProxy('/ee_rpy_srv', EeRpy)
Each one of these Service Clients will communicate with their corresponding Service Servers. These Services will be defined in a different script (which we will create later), and they will be used to send trajectories to the manipulator arm, and also to get data from the end effector.
Finally, we will need to fill up the variables that are needed by the Gazebo Environment. These are the controllers_list, the robot_name_space, and the reset_controls variables.
self.controllers_list = []
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env.RobotGazeboEnv
super(FetchEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False)
And that’s all for this first part of the script! Let’s move on to the next section, where we will define all the necessary functions for this environment.
Methods needed by the Robot Environment
So, let’s continue with the creation of our script by creating the methods needed by the Robot Environment. These are all the methods that will be particular to this robot (simulation). Let’s have a look at all the functions, and then we’ll comment on them, one by one.
def _check_all_sensors_ready(self):
self._check_joint_states_ready()
rospy.logdebug("ALL SENSORS READY")
def _check_joint_states_ready(self):
self.joints = None
while self.joints is None and not rospy.is_shutdown():
try:
self.joints = rospy.wait_for_message("/joint_states", JointState, timeout=1.0)
rospy.logdebug("Current /joint_states READY=>" + str(self.joints))
except:
rospy.logerr("Current /joint_states not ready yet, retrying for getting joint_states")
return self.joints
def joints_callback(self, data):
self.joints = data
def get_joints(self):
return self.joints
def set_trajectory_ee(self, action):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
ee_target = EeTrajRequest()
ee_target.pose.orientation.w = 1.0
ee_target.pose.position.x = action[0]
ee_target.pose.position.y = action[1]
ee_target.pose.position.z = action[2]
result = self.ee_traj_client(ee_target)
return True
def set_trajectory_joints(self, initial_qpos):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
joint_point = JointTrajRequest()
joint_point.point.positions = [None] * 7
joint_point.point.positions[0] = initial_qpos["joint0"]
joint_point.point.positions[1] = initial_qpos["joint1"]
joint_point.point.positions[2] = initial_qpos["joint2"]
joint_point.point.positions[3] = initial_qpos["joint3"]
joint_point.point.positions[4] = initial_qpos["joint4"]
joint_point.point.positions[5] = initial_qpos["joint5"]
joint_point.point.positions[6] = initial_qpos["joint6"]
result = self.joint_traj_client(joint_point)
return True
def get_ee_pose(self):
gripper_pose_req = EePoseRequest()
gripper_pose = self.ee_pose_client(gripper_pose_req)
return gripper_pose
def get_ee_rpy(self):
gripper_rpy_req = EeRpyRequest()
gripper_rpy = self.ee_rpy_client(gripper_rpy_req)
return gripper_rpy
The _check_all_sensors_ready() function, in this case, is just calling the _check_joint_states_ready() function, which is the important one. This function checks that the /joint_states topic is being published.
self.joints = rospy.wait_for_message("/joint_states", JointState, timeout=1.0)
We need to make sure that this topic is being published because it is the one that will give us data about the state of the joints at every moment.
The joints_callback() function is the callback for the Subscriber that we declared in the initialization of the class (self.joint_states_sub). It just stores the data from the /joint_states topic into the variable of the class, self.joints.
The get_joints() function simply returns this self.joints variable, which is filled in the above callback function.
The set_trajectory_ee (self, action) function is one of the most important, as it will allow us to send trajectories to the manipulator arm. For this function, we are getting a variable named actionas input. This action variable will contain the coordinates (x, y, z) of the desired position for the end effector of the manipulator. Once we have this position for the end effector, we send it through the Service Client that we defined in the initialization of the class (self.ee_traj_client). This will activate the Service Server that is in charge of executing the trajectory in the robot (we’ll see this Service Server later in the unit).
def set_trajectory_ee(self, action):
ee_target = EeTrajRequest()
ee_target.pose.orientation.w = 1.0
ee_target.pose.position.x = action[0]
ee_target.pose.position.y = action[1]
ee_target.pose.position.z = action[2]
result = self.ee_traj_client(ee_target)
return True
The set_trajectory_joints (self, initial_qpos) function is quite similar to the one we’ve just seen. It also sends a trajectory to be executed by the manipulator, but in this case, the goal is a set of joint positions instead of the desired end effector position. We get these joint positions from the input variable initial_qpos, as you can see in the code below.
def set_trajectory_joints(self, initial_qpos):
joint_point = JointTrajRequest()
joint_point.point.positions = [None] * 7
joint_point.point.positions[0] = initial_qpos["joint0"]
joint_point.point.positions[1] = initial_qpos["joint1"]
joint_point.point.positions[2] = initial_qpos["joint2"]
joint_point.point.positions[3] = initial_qpos["joint3"]
joint_point.point.positions[4] = initial_qpos["joint4"]
joint_point.point.positions[5] = initial_qpos["joint5"]
joint_point.point.positions[6] = initial_qpos["joint6"]
result = self.joint_traj_client(joint_point)
return True
Finally, we have the get_ee_pose and get_ee_rpy functions. These functions are used to get data from the position (x, y, z) and the roll, pitch, and yaw of the end effector. For this, they also use the Service Client defined in the initialization of the class, self.ee_pose_client and self.ee_rpy_client. We will see the Service Servers related to them later in the unit.
And that’s it! With the above methods, we will be able to do all the basic interactions with our Fetch robot, regardless of the kind of task for which we want to train the robot.
Methods needed by the Gazebo Environment
Now, let’s have a quick look at the methods that we will need to define for the Gazebo Environment. As you may already know from the previous chapter, the Gazebo Environment requires that we define a function called _check_all_systems_ready(self). This function will be in charge of checking that everything we need is working fine.
def _check_all_systems_ready(self):
self._check_all_sensors_ready()
return True
As you can see, in this case, the only thing we are doing here is calling the checkall_sensors_ready(). And, as you saw in the previous section, this function checks if the topic /joint_states is being published correctly. This is all we need to do here. Quite simple, right?
Methods needed by the Task Environment
Finally, as you should already know from the previous chapter, we have all the defined methods that are needed in the Task Environment, which we will see in the next chapter.
def _init_env_variables(self):
"""Inits variables needed to be initialised each time we reset at the start
of an episode.
"""
raise NotImplementedError()
def _compute_reward(self, observations, done):
"""Calculates the reward to give based on the observations given.
"""
raise NotImplementedError()
def _set_action(self, action):
"""Applies the given action to the simulation.
"""
raise NotImplementedError()
def _get_obs(self):
raise NotImplementedError()
def _is_done(self, observations):
"""Checks if episode done based on observations given.
"""
raise NotImplementedError()
There’s no need to modify anything from the template here.
And that’s (almost) everything for the Robot Environment. Until now, we’ve always worked with the Gazebo Environment, which is defined in the robot_gazebo_env.py file. But that’s not the only type of Gazebo Environment that exists. So, let me introduce you to the Gazebo Goal Environment.
The Gazebo Goal Environment
First of all, let me say that there’s no need to panic about this new environment that I’m introducing. The Gazebo Goal Environment is exactly the same as the regular Gazebo Environment; the ONLY difference is the environment they inherit from.
The regular Gazebo Environment inherits from the gym.Env environment:
class RobotGazeboEnv(gym.Env):
while the Gazebo Goal Environment inherits from the gym.GoalEnv environment:
class RobotGazeboEnv(gym.GoalEnv):
And this is the only difference between the two of them. Everything else is exactly the same.
But now you may be wondering… what’s this Goal Environment? Let me explain it as simply as possible.
Basically, regular environments are made to work with one goal, which is always the same. For instance, with the CartPole example, the goal was to keep the pole upright. This goal is ALWAYS the same, and it doesn’t change throughout the whole training. In a Goal-based environment, this concept is different.
Basically, Goal-based environments allow the modification of the goal during the training. And this is very useful for learning algorithms, like HER, which is the one we are going to use for the Fetch robot. Once we explain the basics of the HER algorithm in the following chapter, you will surely understand why we are using this Goal Environment, instead of the regular one.
So, in order to use this Goal Environment, we will need to do a couple of modifications to our Robot Environment.
| Exercise 7.2 |
|---|
a) Modify the class definition so that it now uses the Goal Environment. The line you will need to change is the following:
class FetchEnv(robot_gazebo_env.RobotGazeboEnv):
b) Remember to also import the correct file that contains the Goal Environment.
from my_fetch_train import robot_gazebo_env_goal
c) Also, remember to place inside your package the Gazebo Goal Environment. Below you can see an example of a Goal Environment:
| robot_gazebo_env_goal.py |
|---|
import rospy
import gym
from gym.utils import seeding
from .gazebo_connection import GazeboConnection
from .controllers_connection import ControllersConnection
#https://bitbucket.org/theconstructcore/theconstruct_msgs/src/master/msg/RLExperimentInfo.msg
from theconstruct_msgs.msg import RLExperimentInfo
class RobotGazeboEnv(gym.GoalEnv):
def __init__(self, robot_name_space, controllers_list, reset_controls):
# To reset Simulations
print ("Entered Gazebo Env")
self.gazebo = GazeboConnection(start_init_physics_parameters=False, reset_world_or_sim="WORLD")
self.controllers_object = ControllersConnection(namespace=robot_name_space, controllers_list=controllers_list)
self.reset_controls = reset_controls
print (self.reset_controls)
self.seed()
# Set up ROS related variables
self.episode_num = 0
self.reward_pub = rospy.Publisher('/openai/reward', RLExperimentInfo, queue_size=1)
# Env methods
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def step(self, action):
"""
Function executed each time step.
Here we get the action execute it in a time step and retrieve the
observations generated by that action.
:param action:
:return: obs, reward, done, info
"""
"""
Here we should convert the action num to movement action, execute the action in the
simulation and get the observations result of performing that action.
"""
print ("Entered step")
print ("Unpause sim")
self.gazebo.unpauseSim()
print ("Set action")
print ("Action:")
print (action)
self._set_action(action)
print ("Get Obs")
obs = self._get_obs()
print ("Is done")
done = self._is_done(obs)
info = {}
reward = self._compute_reward(obs, done)
self._publish_reward_topic(reward, self.episode_num)
return obs, reward, done, info
def reset(self):
rospy.logdebug("Reseting RobotGazeboEnvironment")
print ("Entered reset")
self._reset_sim()
self._init_env_variables()
self._update_episode()
obs = self._get_obs()
return obs
def close(self):
"""
Function executed when closing the environment.
Use it for closing GUIS and other systems that need closing.
:return:
"""
rospy.logdebug("Closing RobotGazeboEnvironment")
rospy.signal_shutdown("Closing RobotGazeboEnvironment")
def _update_episode(self):
"""
Increases the episode number by one
:return:
"""
self.episode_num += 1
def _publish_reward_topic(self, reward, episode_number=1):
"""
This function publishes the given reward in the reward topic for
easy access from ROS infrastructure.
:param reward:
:param episode_number:
:return:
"""
reward_msg = RLExperimentInfo()
reward_msg.episode_number = episode_number
reward_msg.episode_reward = reward
self.reward_pub.publish(reward_msg)
# Extension methods
# ----------------------------
def _reset_sim(self):
"""Resets a simulation
"""
if self.reset_controls:
self.gazebo.unpauseSim()
self.controllers_object.reset_controllers()
self._check_all_systems_ready()
self._set_init_pose()
self.gazebo.pauseSim()
self.gazebo.resetSim()
self.gazebo.unpauseSim()
self.controllers_object.reset_controllers()
self._check_all_systems_ready()
self.gazebo.pauseSim()
else:
self.gazebo.unpauseSim()
self._check_all_systems_ready()
self._set_init_pose()
self.gazebo.resetWorld()
self._check_all_systems_ready()
return True
def _set_init_pose(self):
"""Sets the Robot in its init pose
"""
raise NotImplementedError()
def _check_all_systems_ready(self):
"""
Checks that all the sensors, publishers and other simulation systems are
operational.
"""
raise NotImplementedError()
def _get_obs(self):
"""Returns the observation.
"""
raise NotImplementedError()
def _init_env_variables(self):
"""Inits variables needed to be initialised each time we reset at the start
of an episode.
"""
raise NotImplementedError()
def _set_action(self, action):
"""Applies the given action to the simulation.
"""
raise NotImplementedError()
def _is_done(self, observations):
"""Indicates whether or not the episode is done ( the robot has fallen for example).
"""
raise NotImplementedError()
def _compute_reward(self, observations, done):
"""Calculates the reward to give based on the observations given.
"""
raise NotImplementedError()
def _env_setup(self, initial_qpos):
"""Initial configuration of the environment. Can be used to configure initial state
and extract information from the simulation.
"""
raise NotImplementedError()
| End of Exercise 7.2 |
|---|
Summary
Great! So, if you’ve followed all the steps correctly, you should have something like this in the end:
| fetch_env_v2.py |
|---|
import numpy
import rospy
from openai_ros import robot_gazebo_env_goal
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from nav_msgs.msg import Odometry
from fetch_train.srv import EePose, EePoseRequest, EeRpy, EeRpyRequest, EeTraj, EeTrajRequest, JointTraj, JointTrajRequest
class FetchEnv(robot_gazebo_env_goal.RobotGazeboEnv):
"""Superclass for all Fetch environments.
"""
def __init__(self):
print ("Entered Fetch Env")
"""Initializes a new Fetch environment.
Args:
"""
"""
To check any topic we need to have the simulations running, we need to do two things:
1) Unpause the simulation: without that the stream of data doesn't flow. This is for simulations
that are paused for whatever reason
2) If the simulation was running already for some reason, we need to reset the controllers.
This has to do with the fact that some plugins with tf don't understand the reset of the simulation
and need to be reset to work properly.
"""
# We Start all the ROS related Subscribers and publishers
JOINT_STATES_SUBSCRIBER = '/joint_states'
self.joint_states_sub = rospy.Subscriber(JOINT_STATES_SUBSCRIBER, JointState, self.joints_callback)
self.joints = JointState()
self.ee_traj_client = rospy.ServiceProxy('/ee_traj_srv', EeTraj)
self.joint_traj_client = rospy.ServiceProxy('/joint_traj_srv', JointTraj)
self.ee_pose_client = rospy.ServiceProxy('/ee_pose_srv', EePose)
self.ee_rpy_client = rospy.ServiceProxy('/ee_rpy_srv', EeRpy)
# Variables that we give through the constructor.
self.controllers_list = []
self.robot_name_space = ""
# We launch the init function of the Parent Class robot_gazebo_env_goal.RobotGazeboEnv
super(FetchEnv, self).__init__(controllers_list=self.controllers_list,
robot_name_space=self.robot_name_space,
reset_controls=False)
# RobotGazeboEnv virtual methods
# ----------------------------
def _check_all_systems_ready(self):
"""
Checks that all the sensors, publishers and other simulation systems are
operational.
"""
self._check_all_sensors_ready()
return True
# FetchEnv virtual methods
# ----------------------------
def _check_all_sensors_ready(self):
self._check_joint_states_ready()
rospy.logdebug("ALL SENSORS READY")
def _check_joint_states_ready(self):
self.joints = None
while self.joints is None and not rospy.is_shutdown():
try:
self.joints = rospy.wait_for_message("/joint_states", JointState, timeout=1.0)
rospy.logdebug("Current /joint_states READY=>" + str(self.joints))
except:
rospy.logerr("Current /joint_states not ready yet, retrying for getting joint_states")
return self.joints
def joints_callback(self, data):
self.joints = data
def get_joints(self):
return self.joints
def set_trajectory_ee(self, action):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
ee_target = EeTrajRequest()
ee_target.pose.orientation.w = 1.0
ee_target.pose.position.x = action[0]
ee_target.pose.position.y = action[1]
ee_target.pose.position.z = action[2]
result = self.ee_traj_client(ee_target)
return True
def set_trajectory_joints(self, initial_qpos):
"""
Helper function.
Wraps an action vector of joint angles into a JointTrajectory message.
The velocities, accelerations, and effort do not control the arm motion
"""
# Set up a trajectory message to publish.
joint_point = JointTrajRequest()
joint_point.point.positions = [None] * 7
joint_point.point.positions[0] = initial_qpos["joint0"]
joint_point.point.positions[1] = initial_qpos["joint1"]
joint_point.point.positions[2] = initial_qpos["joint2"]
joint_point.point.positions[3] = initial_qpos["joint3"]
joint_point.point.positions[4] = initial_qpos["joint4"]
joint_point.point.positions[5] = initial_qpos["joint5"]
joint_point.point.positions[6] = initial_qpos["joint6"]
result = self.joint_traj_client(joint_point)
return True
def get_ee_pose(self):
gripper_pose_req = EePoseRequest()
gripper_pose = self.ee_pose_client(gripper_pose_req)
return gripper_pose
def get_ee_rpy(self):
gripper_rpy_req = EeRpyRequest()
gripper_rpy = self.ee_rpy_client(gripper_rpy_req)
return gripper_rpy
# ParticularEnv methods
# ----------------------------
def _init_env_variables(self):
"""Inits variables needed to be initialised each time we reset at the start
of an episode.
"""
raise NotImplementedError()
def _compute_reward(self, observations, done):
"""Calculates the reward to give based on the observations given.
"""
raise NotImplementedError()
def _set_action(self, action):
"""Applies the given action to the simulation.
"""
raise NotImplementedError()
def _get_obs(self):
raise NotImplementedError()
def _is_done(self, observations):
"""Checks if episode done based on observations given.
"""
raise NotImplementedError()
| fetch_env_v2.py |
|---|
Now, there’s only one thing remaining. We need to create our script to place all the Service Servers that we’ll use to interact with the manipulator and the end-effector. So, let’s go for it!
Support Script for Manipulator Arm
There’s one question you may be asking yourself: why do we need to create this extra script, instead of putting everything inside the Robot Environment? And that’s a very good question. Let me try to answer it.
For this Fetch training, we are going to use the HER algorithm, which is included in OpenAI baselines, just as the deepq algorithm that you’ve already used in the course is. So, to be able to use the HER algorithm, we will also need to launch it from a Python 3.5 virtual environment. And this can create some problem for ROS, since ROS only officially supports Python 2.7.
In this case, the problem we face is that for Python 3.5, we don’t have some of the ROS modules that are required for interacting with the MoveIt API available (which allows us to control the manipulator). So, we have to create a little “trick” here in order to make it work. Basically, what we have done is separate all the functions that have to interact with this ROS module from the environments structure, and place them into a separate script. This way, for the part of code that will be executed with Python 3.5, we will only have Service Clients, which will perform calls to those functionalities that are actually in a separate script. This script, then, will be executed outside of the Python 3.5 virtual environment to avoid all those compatibility problems.
And that’s basically it. So, let’s have a look at the code of this script, and then we’ll comment a little bit on the most important parts.
| execute_trajectories.py |
|---|
#! /usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import trajectory_msgs.msg
from fetch_train.srv import EePose, EePoseResponse, EeRpy, EeRpyResponse, EeTraj, EeTrajResponse, JointTraj, JointTrajResponse
class ExecTrajService(object):
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
self.ee_traj_srv = rospy.Service('/ee_traj_srv', EeTraj , self.ee_traj_callback)
self.joint_traj_srv = rospy.Service('/joint_traj_srv', JointTraj , self.joint_traj_callback)
self.ee_pose_srv = rospy.Service('/ee_pose_srv', EePose , self.ee_pose_callback)
self.ee_rpy_srv = rospy.Service('/ee_rpy_srv', EeRpy , self.ee_rpy_callback)
self.pose_target = geometry_msgs.msg.Pose()
def ee_traj_callback(self, request):
self.pose_target.orientation.w = request.pose.orientation.w
self.pose_target.position.x = request.pose.position.x
self.pose_target.position.y = request.pose.position.y
self.pose_target.position.z = request.pose.position.z
self.group.set_pose_target(self.pose_target)
self.execute_trajectory()
response = EeTrajResponse()
response.success = True
response.message = "Everything went OK"
return response
def joint_traj_callback(self, request):
self.group_variable_values = self.group.get_current_joint_values()
print ("Group Vars:")
print (self.group_variable_values)
print ("Point:")
print (request.point.positions)
self.group_variable_values[0] = request.point.positions[0]
self.group_variable_values[1] = request.point.positions[1]
self.group_variable_values[2] = request.point.positions[2]
self.group_variable_values[3] = request.point.positions[3]
self.group_variable_values[4] = request.point.positions[4]
self.group_variable_values[5] = request.point.positions[5]
self.group_variable_values[6] = request.point.positions[6]
self.group.set_joint_value_target(self.group_variable_values)
self.execute_trajectory()
response = JointTrajResponse()
response.success = True
response.message = "Everything went OK"
return response
def execute_trajectory(self):
self.plan = self.group.plan()
self.group.go(wait=True)
def ee_pose_callback(self, request):
gripper_pose = self.group.get_current_pose()
gripper_pose_res = EePoseResponse()
gripper_pose_res = gripper_pose.pose
return gripper_pose_res
def ee_rpy_callback(self, request):
gripper_rpy = self.group.get_current_rpy()
gripper_rpy_res = EeRpyResponse()
gripper_rpy_res.r = gripper_rpy[0]
gripper_rpy_res.y = gripper_rpy[1]
gripper_rpy_res.p = gripper_rpy[2]
return gripper_rpy_res
if __name__ == "__main__":
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
traj_serv_object = ExecTrajService()
rospy.spin() # mantain the service open.
| execute_trajectories.py |
|---|
First of all, we can see the initialization of the class. Here we are basically initializing the moveit_commander, which will allow us to communicate with the MoveIt RViz interface:
moveit_commander.roscpp_initialize(sys.argv)
And also initializing some other objects, like the MoveGroupCommander, which is an interface to the manipulator group of joints that we want to control. You can find more precise information on this in the ROS Manipulation in 5 Days Course, if you are interested.
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
Then, we are also creating the Services that we will use to send motions to our robot.
self.ee_traj_srv = rospy.Service(’/ee_traj_srv’, EeTraj , self.ee_traj_callback) self.joint_traj_srv = rospy.Service(’/joint_traj_srv’, JointTraj , self.joint_traj_callback) self.ee_pose_srv = rospy.Service(’/ee_pose_srv’, EePose , self.ee_pose_callback) self.ee_rpy_srv = rospy.Service(’/ee_rpy_srv’, EeRpy , self.ee_rpy_callback)
Next, we can find the callback functions of all our services. The ee_traj_callback (self, request) will be in charge of calculating trajectories that are based on end effector goal positions.
def ee_traj_callback(self, request):
self.pose_target.orientation.w = request.pose.orientation.w
self.pose_target.position.x = request.pose.position.x
self.pose_target.position.y = request.pose.position.y
self.pose_target.position.z = request.pose.position.z
self.group.set_pose_target(self.pose_target)
self.execute_trajectory()
response = EeTrajResponse()
response.success = True
response.message = "Everything went OK"
return response
This function will receive a desired end-effector position in the request variable. This request variable will come from the Service Client call, which is in the set_trajectory_ee (self, action):function from the Robot Environment. Then, it will calculate the best motion to reach the desired position, and it will execute this trajectory by calling to the execute_trajectory() function.
Next, we have the joint_traj_callback (self, request) function.
def joint_traj_callback(self, request):
self.group_variable_values = self.group.get_current_joint_values()
print ("Group Vars:")
print (self.group_variable_values)
print ("Point:")
print (request.point.positions)
self.group_variable_values[0] = request.point.positions[0]
self.group_variable_values[1] = request.point.positions[1]
self.group_variable_values[2] = request.point.positions[2]
self.group_variable_values[3] = request.point.positions[3]
self.group_variable_values[4] = request.point.positions[4]
self.group_variable_values[5] = request.point.positions[5]
self.group_variable_values[6] = request.point.positions[6]
self.group.set_joint_value_target(self.group_variable_values)
self.execute_trajectory()
response = JointTrajResponse()
response.success = True
response.message = "Everything went OK"
return response
This function is basically the same as the previous one, but instead of setting the goal from an end effector position, it will set it from specific joint positions. This function will be called by the set_trajectory_joints (self, initial_qpos) function from the Robot Environment.
Next, we have the execute_trajectory (self) function.
def execute_trajectory(self):
self.plan = self.group.plan()
self.group.go(wait=True)
This function simply executes the trajectory that has been calculated by MoveIt!.
Finally, we have the ee_pose_callback (self, request) and ee_rpy_callback (self, request) functions. These functions will return the pose (x, y, z) and the rpy (roll, pitch, yaw) of the end-effector, respectively. Their related functions in the Robot Environment are get_ee_pose (self) and get_ee_rpy (self).
def ee_pose_callback(self, request):
gripper_pose = self.group.get_current_pose()
gripper_pose_res = EePoseResponse()
gripper_pose_res = gripper_pose.pose
return gripper_pose_res
def ee_rpy_callback(self, request):
gripper_rpy = self.group.get_current_rpy()
gripper_rpy_res = EeRpyResponse()
gripper_rpy_res.r = gripper_rpy[0]
gripper_rpy_res.y = gripper_rpy[1]
gripper_rpy_res.p = gripper_rpy[2]
return gripper_rpy_res
| Exercise 7.3 |
|---|
a) Create a new package, named fetch_train, where you’ll place all the support scripts and files that you’ll need.
b) Inside this new package, create a new folder named src, and paste into there the support script that we have just reviewed.
c) Also, inside the package, you will need to create and compile 4 new Service messages, that will be used to communicate between your support script (Python 2.7), and the Fetch environments (Python 3.5). Below you will see the .srv files of each one of them. If you are not sure about how to create new Service Messages, you should have a look at the Unit 6 - ROS Services Part 2 of the ROS Basics in 5 Days Course, where this is explained in detail.
| EePose.srv |
|---|
---
geometry_msgs/Pose pose
| EeRpy.srv |
|---|
---
float32 r
float32 p
float32 y
| EeTraj.srv |
|---|
geometry_msgs/Pose pose
---
bool success
string message
| JointTraj.srv |
|---|
trajectory_msgs/JointTrajectoryPoint point
---
bool success
string message
| End of Exercise 7.3 |
|---|
And that’s basically it! There’s just one more thing you need to know. In order to be able to use the moveit_commander module (which is basically an interface to MoveIt!), you need to have MoveIt! running first. And in order to have MoveIt! running, you will need to create a MoveIt! package for the Fetch robot.
Do you already know how to do that? Great! Then just take a moment to create a MoveIt pacakge for our Fetch robot. Anyways, in case you don’t how to do that, don’t panic! You can have a look at the following video where everything is explained!
(上方的视频是油管链接，国内似乎还没有人上传过)
Also, learning about MoveIt is not the goal of this Course, but if you are interested in learning more about MoveIt!, you can have a look at our ROS Manipulation in 5 Days, where you will find more complete tutorials about MoveIt and Manipulation, which might help you in this example.
And remember, BE AWARE that you’ll need to launch the MoveIt package before using this support script script.
4156
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
