rpapallas
commented
5 years ago Can you post the code you are using that produces this error?
Closed lakshmip001 closed 2 years ago
rpapallas
commented
5 years ago Can you post the code you are using that produces this error?
lakshmip001
commented
5 years ago in ur5_factory.py I commented out gripper elements.
#!/usr/bin/env python
# Copyright (C) 2018 The University of Leeds.
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
"""
Include a factory class to generate UR5 instances.
UR5 instances can be generated with different configuration. For example
the robot may have a ClearPath ridgeback, a two finger gripper etc where
in another setup the UR5 could not have the ridgeback, and could have
a three finger gripper instead.
This file define a class that let you create different robot configurations
on the fly.
"""
__author__ = "Rafael Papallas"
__authors__ = ["Rafael Papallas"]
__copyright__ = "Copyright (C) 2018, The University of Leeds"
__credits__ = ["Rafael Papallas", "Dr. Mehmet Dogar"]
__email__ = "Rafael: r.papallas@leeds.ac.uk | Mehmet: m.r.dogar@leeds.ac.uk"
__license__ = "GPLv3"
__maintainer__ = "Rafael Papallas"
__status__ = "Production"
__version__ = "0.0.1"
import rospy
from openravepy import Environment
from openravepy import RaveCreateIkSolver
from openravepy import RaveCreateModule
from openravepy import RaveInitialize
from openravepy import RaveLogInfo
from openravepy import RaveLogWarn
from ur5_robot import UR5_Robot
class UR5_Factory(object):
"""
Class for generating UR5 robot configurations and environments.
This class builds a UR5 robot by loading it from URDF files and
also adds some other implementation details that are required. This
class should be used to create UR5 instances in your OpenRAVE
programs.
"""
def __init__(self):
"""Initialise the UR5_Factory.
Initialise the class by defining available grippers and
viewers.
"""
# TODO: Add support for robotiq_three_finger
self._available_grippers = ["robotiq_two_finger"]
def create_ur5_and_env(self, is_simulation=True,
has_ridgeback=True,
#gripper_name="robotiq_two_finger",
has_force_torque_sensor=True,
env_path=None,
viewer_name="qtcoin",
urdf_path="/home/lakshmi/cat_OR_ws/src/universal_robot/ur_description/urdf/",
srdf_path="/home/lakshmi/cat_OR_ws/src/universal_robot/ur_description/urdf/"):
"""
Create a UR5 and Environment instance.
This method given some specifications as arguments, will build
a UR5 robot instance accordingly.
Args:
is_simulation: Indicates whether the robot is in simulation
or not.
has_ridgeback: Indicates whether the returned robot model
has a ClearPath Ridgeback moving base
integrated with it.
gripper_name: The gripper name to be used in the robot model
among a list of available grippers.
has_force_torque_sensor: Indicates whether the robot model
will have a Robotiq Force Torque s
ensor attached to it.
env_path: The environment XML file to load from.
viewer_name: The viewer name (e.g qtcoin, rviz etc) among
a list of available names.
urdf_path: The path of the URDF files if you moved the URDFs
to another location, if you haven't, then leave this
to default.
srdf_path: The path of the SRDF files if you moved the SRDFs
to another location, if you haven't, then leave this
to default.
Returns:
An openravepy.Environment and openravepy.Robot instances.
Raises:
ValueError: If gripper_name is invalid invalid.
"""
#if gripper_name not in self._available_grippers:
# raise ValueError("Gripper {} is not supported. The only " \
# "available gripper names are: " \
# "{}".format(gripper_name,
# ', '.join(self._available_grippers)))
# TODO: Create URDFs that do not include the ridgeback.
if not has_ridgeback:
raise NotImplementedError("Robot configuration without ridgeback " \
"is not yet available. Robot configuration " \
"without the Clearpath Ridgeback is under " \
"development at the moment (please check " \
"that you also have the latest version " \
"of this code).")
RaveInitialize(True)
self.env = self._create_environment(env_path)
self.robot = self._load_ur5_from_urdf( has_ridgeback,
has_force_torque_sensor, urdf_path,
srdf_path)
if not self._a_ros_topic_exist_with_the_name("joint_states"):
is_simulation = True
# Add class UR5_Robot to the robot.
self.robot.__class__ = UR5_Robot
self.robot.__init__(is_simulation)
# Attach controllers
if not is_simulation:
self._attach_robot_controller()
#self._attach_gripper_controller(gripper_name)
# Required for or_rviz to work with the robot's interactive marker.
self._set_ik_solver()
self._set_viewer(viewer_name)
return self.env, self.robot
def _set_ik_solver(self):
ik_solver = RaveCreateIkSolver(self.env, self.robot.ikmodel.getikname())
self.robot.manipulator.SetIkSolver(ik_solver)
def _attach_gripper_controller(self, gripper_name):
if gripper_name == "robotiq_two_finger":
controller_name = "robotiqcontroller"
if self._a_ros_topic_exist_with_the_name("CModelRobotInput"):
self.robot.attach_controller(name=controller_name, dof_indices=[3])
RaveLogInfo("robotiq_two_finger controller attached successfully.")
else:
RaveLogWarn("End-effector controller not attached, " \
"topics ('CModelRobotInput' or/and " \
"'CModelRobotOutput') are not available.")
def _attach_robot_controller(self):
# This is a defensive mechanism to avoid IsDone() method of the
# end-effector controller block the program execution. For further
# discussion, see this thread: https://stackoverflow.com/questions/49552755/openrave-controllerbase-is-blocking-at-the-isdone-method-and-never-returns/49552756#49552756
if self._a_ros_topic_exist_with_the_name("joint_states"):
self.robot.attach_controller(name="ur5controller",
dof_indices=[2, 1, 0, 4, 5, 6])
RaveLogInfo("UR5 controller attached successfully.")
else:
RaveLogWarn("UR5 Controller not attached to robot. Required " \
"topics not being published, rolling back to " \
"simulation.")
def _a_ros_topic_exist_with_the_name(self, topic_name):
# The ROS topic name always start with '/' character.
if topic_name[0] != "/":
topic_name = "/" + topic_name
# List of all published ROS topics
topics = rospy.get_published_topics()
for topic in topics:
if topic[0] == topic_name:
return True
return False
def _get_file_name_from_specification(self, has_ridgeback,
has_force_torque_sensor):
"""
Return the correct URDF file given some configuration specification.
Different robot configurations (with ridgeback and two-finger gripper
or without ridgeback and with three-finger gripper) exists with
different URDF and SRDF files. Hence this method given some
specifications will return the appropriate URDF file from the
package.
Args:
gripper_name: The gripper name to be loaded.
has_ridgeback: Whether ClearPath Ridgeback will be loaded with
the robot model.
has_force_torque_sensor: Whether Robotiq force torque sensor
will be loaded with the robot model.
Returns:
A file name to a URDF/SRDF file matching the configuration
specified by the arguments.
"""
#if gripper_name == "robotiq_two_finger" and has_ridgeback and has_force_torque_sensor:
# return "clearpath_ridgeback__ur5__robotiq_two_finger_gripper__robotiq_fts150"
#if gripper_name == "robotiq_two_finger" and has_ridgeback and not has_force_torque_sensor:
# return "clearpath_ridgeback__ur5__robotiq_two_finger_gripper"
#if gripper_name == "robotiq_three_finger" and has_ridgeback and has_force_torque_sensor:
# return "clearpath_ridgeback__ur5__robotiq_three_finger_gripper__robotiq_fts150"
#if gripper_name == "robotiq_three_finger" and has_ridgeback and not has_force_torque_sensor:
# return "clearpath_ridgeback__ur5__robotiq_three_finger_gripper"
return "ur5"
def _load_ur5_from_urdf(self, has_ridgeback,
has_force_torque_sensor, urdf_path, srdf_path):
"""
Load a UR5 robot model from URDF to the environment.
Given the appropriate configuration and valid path to URDF
and SRDF files, will load UR5 in OpenRAVE environment and will
return a robot instance back.
Args:
gripper_name: The gripper name to be loaded.
has_ridgeback: Whether ClearPath Ridgeback will be loaded with
the robot model.
has_force_torque_sensor: Whether Robotiq force torque sensor
will be loaded with the robot model.
urdf_path: The path of the URDF files.
srdf_path: The path of the SRDF files.
Returns:
A UR5 openravepy.Robot instance.
Raises:
Exception: If or_urdf module cannot be loaded, or something
went wrong while trying to load robot from a URDF file,
or the robot couldn't be loaded from file.
"""
file_name = self._get_file_name_from_specification(has_ridgeback,
has_force_torque_sensor)
urdf_path = urdf_path + "{}.urdf".format(file_name)
srdf_path = srdf_path + "{}.srdf".format(file_name)
urdf_module = RaveCreateModule(self.env, 'urdf')
if urdf_module is None:
raise Exception("Unable to load or_urdf module. Make sure you " \
"have or_urdf installed in your Catkin " \
"check: " \
"https://github.com/personalrobotics/or_urdf")
ur5_name = urdf_module.SendCommand('LoadURI {} {}'.format(urdf_path,
srdf_path))
if ur5_name is None:
raise Exception("Something went wrong while trying to load " \
"UR5 from file. Is the path correct?")
robot = self.env.GetRobot(ur5_name)
if robot is None:
raise Exception("Unable to find robot with " \
"name '{}'.".format(ur5_name))
return robot
def _create_environment(self, env_path):
"""
Create an Environment instance and load an XML environment to it.
Will create an openravepy.Environment instance and will try to
load an XML environment specification from file.
Args:
env_path: An XML file to load an environment.
Returns:
The openravepy.Environment instance.
Raises:
ValueError: If unable to load environment from file.
"""
env = Environment()
if env_path is not None:
if not env.Load(env_path):
raise ValueError("Unable to load environment " \
"file: '{}'".format(env_path))
return env
def _set_viewer(self, viewer_name):
"""
Set the viewer using the user-specified viewer_name.
Args:
viewer_name: The name of viewer to be set.
Raises:
Exception: If there was a problem setting the viewer.
"""
self.env.SetViewer(viewer_name)
if self.env.GetViewer() is None:
raise Exception("There was something wrong when loading " \
"the {} viewer.".format(viewer_name))control_ur5.py removed the gripper components:
#!/usr/bin/env python
# Copyright (C) 2017 The University of Leeds
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
import IPython
import numpy
from openravepy import IkFilterOptions
from openravepy import matrixFromAxisAngle
from openravepy import PlanningError
from openravepy import *
from openravepy.misc import InitOpenRAVELogging
from ur5_factory import UR5_Factory
import raveutils as ru
__author__ = "Rafael Papallas"
__authors__ = ["Rafael Papallas"]
__copyright__ = "Copyright (C) 2018, The University of Leeds"
__credits__ = ["Rafael Papallas", "Dr. Mehmet Dogar"]
__email__ = "Rafael: r.papallas@leeds.ac.uk | Mehmet: m.r.dogar@leeds.ac.uk"
__license__ = "GPLv3"
__maintainer__ = "Rafael Papallas"
__status__ = "Production"
__version__ = "0.0.1"
class Demo:
def __init__(self):
ur5_factory = UR5_Factory()
# The create_ur5_and_env() method takes also the following optional
# arguments: is_simulation, has_ridgeback, gripper_name,
# has_force_torque_sensor, env_path, viewer_name, urdf_path, and
# srdf_path. See ur5_factory.py class for more details.
self.env, self.robot = ur5_factory.create_ur5_and_env(env_path="test1_env.xml",
is_simulation=False)
self.move_robot_to_start_transform()
def move_robot_to_start_transform(self):
# Move manipulator to the start transform (just above the table
# surface)
start_transform = numpy.eye(4)
start_transform[0, 3] = 0.80917
start_transform[1, 3] = 0.05212
start_transform[2, 3] = 0.81
ik_solution = self.robot.manipulator.FindIKSolution(start_transform, IkFilterOptions.CheckEnvCollisions)
self.robot.base_manipulation.MoveManipulator(goal=ik_solution)
def is_trajectory_safe(self, trajectory_object):
waypoints = trajectory_object.GetAllWaypoints2D()
waypoint_sums = [0, 0, 0, 0, 0, 0]
for i in range(0, len(waypoints) - 1):
for j in range(0, 6):
difference = abs(waypoints[i][j] - waypoints[i+1][j])
waypoint_sums[j] += difference
waypoint_below_threshold = [x < numpy.pi/3 for x in waypoint_sums]
return all(waypoint_below_threshold)
def rotate_hand(self, rotation_matrix):
current_hand_transform = self.robot.end_effector_transform
goal_transform = numpy.dot(current_hand_transform, rotation_matrix)
ik_solution = self.robot.manipulator.FindIKSolution(goal_transform, IkFilterOptions.CheckEnvCollisions)
if ik_solution is not None:
try:
trajectory_object = self.robot.base_manipulation.MoveManipulator(goal=ik_solution, execute=False, outputtrajobj=True)
except PlanningError, e:
print "There was a planning error"
print e
if trajectory_object and self.is_trajectory_safe(trajectory_object):
self.robot.execute_trajectory_and_wait_for_controller(trajectory_object)
else:
print "The trajectory failed the safety check."
else:
print "No IK Solution found."
def move_hand(self, x_offset, y_offset):
current_hand_transform = self.robot.end_effector_transform
current_hand_transform[0, 3] += x_offset
current_hand_transform[1, 3] += y_offset
ik_solution = self.robot.manipulator.FindIKSolution(current_hand_transform, IkFilterOptions.CheckEnvCollisions)
print "ik_solution"
print ik_solution
dis = [0.01, 0.02, 0.1, 0.5, 1.]
for dist in dis:
while True:
q1 = ru.kinematics.random_joint_values(self.robot)
with self.env:
self.robot.SetActiveDOFValues(q1)
if not self.env.CheckCollision(self.robot):
break
twist = numpy.random.rand(6)*dist
traj = ru.planning.plan_constant_velocity_twist(self.robot, twist, 0.02, 10)
print traj
if ik_solution is not None:
try:
trajectory_object = self.robot.base_manipulation.MoveManipulator(goal=ik_solution, execute=False, outputtrajobj=True)
raveLogInfo('traj has %d waypoints, last waypoint is: %s' % (trajectory_object.GetNumWaypoints(), repr(trajectory_object.GetWaypoint(-1))))
way_points = trajectory_object.GetWaypoints2D(0, trajectory_object.GetNumWaypoints())
print "trajectory_object"
print way_points
planningutils.RetimeTrajectory (trajectory_object, hastimestamps=True, maxvelmult=0.5, maxaccelmult=0.5)
new_waypoints = trajectory_object.GetWaypoints2D(0, trajectory_object.GetNumWaypoints())
print "new_waypoints"
print new_waypoints
except PlanningError, e:
print "There was a planning error"
print e
if trajectory_object and self.is_trajectory_safe(trajectory_object):
self.robot.execute_trajectory_and_wait_for_controller(trajectory_object)
else:
print "The trajectory failed the safety check."
else:
print "No IK Solution found."
def control_robot_with_the_keyboard(self):
OFFSET = 0.4
print ""
print "w: Move arm forward."
print "s: Move arm backwards."
print "a: Move arm left."
print "d: Move arm right."
print "q: Rotate end-effector anticlockwise."
print "e: Rotate end-effector clockwise."
print "o: Open end-effector."
print "c: Close end-effector."
print ""
print "Type 'exit' to stop the demo and enter in IPython environment."
while True:
action = raw_input("Menu Action: ")
if action == "exit": # EXIT Demo
break
if action == "q": # Rotate Anti-clockwise
anticlockwise = matrixFromAxisAngle([0, 0, numpy.pi/6])
self.rotate_hand(anticlockwise)
elif action == "e": # Rotate Clockwise
clockwise = matrixFromAxisAngle([0, 0, -numpy.pi/6])
self.rotate_hand(clockwise)
elif action == "o": # Open Gripper
self.robot.open_gripper()
elif action == "c": # Close Gripper
self.robot.close_gripper()
elif action in ["w", "s", "a", "d"]:
x_offset = 0
y_offset = 0
if action == "w": x_offset = OFFSET # Move Forward
if action == "s": x_offset = -OFFSET # Move Backwards
if action == "a": y_offset = -OFFSET # Move Left
if action == "d": y_offset = OFFSET # Move Right
self.move_hand(x_offset, y_offset)
if __name__ == "__main__":
InitOpenRAVELogging()
demo = Demo()
demo.control_robot_with_the_keyboard()
IPython.embed()I would like to know, how I can use this one to link with the real robot and do planning in a separate program. I tried running this and with separate program planning for motion. I don't see the motion executed on the real robot .(they both are not linked)
rpapallas
commented
5 years ago This is not the way to remove the gripper. You need to create a new URDF and SRDF first and remove the gripper from these files, then you need to modify the files you have posted.
I am planning to create a version where the end-effector is not part of the robot, but it will take a while as we are quite busy now. This could be done anywhere from next week to the end of the month. You could try to do this yourself if you wish.
lakshmip001
commented
5 years ago Ok thanks .I tried using ur5.urdf and ur5.srdf from universal robot package .
rpapallas
commented
5 years ago In regards to your problem connecting the real robot with OpenRAVE, do you use singularity or you are running the controller on the host operating system?
lakshmip001
commented
5 years ago I am running the controller on the host operating system.
lakshmip001
commented
5 years ago I am able to load only UR5 but only problem with UR5 controller. So robot arm doesn't move when trajectory is given to setpath. Kindly help me how I can check if controller is loaded.
rpapallas
commented
5 years ago I am currently working on it. I will let you know the outcome soon.
rpapallas
commented
5 years ago I added support for a UR5 model without a gripper. Please pull the changes first.
The way to use this model is to set the gripper_name=None when creating the ur5 instance:
import IPython
from ur5_factory import UR5_Factory
ur5_factory = UR5_Factory()
# If you want to specify all the configuration settings (is_simulation, has_ridgeback etc)
env, robot = ur5_factory.create_ur5_and_env(is_simulation=True,
has_ridgeback=True,
gripper_name=None,
has_force_torque_sensor=False,
env_path="test_env.xml",
viewer_name="qtcoin",
urdf_path="package://ur5controller/ur5_description/urdf/",
srdf_path="package://ur5controller/ur5_description/srdf/")
IPython.embed()The way to detach the gripper from the model is to explicitly pass None to the gripper_name argument as like in the above example. Note, I have only tested this in simulation and not on a real robot.
Note: It will take a while (around 30 minutes or more) to generate the IK solutions. Please wait for it to finish. If you get any errors during this step, please see the README of this repository, it might include some solutions on how to resolve issues under the Troubleshooting section.
Hope you find this helpful.
Regards, Rafael
robopassio
commented
3 years ago I got the same issues with the latest version install directly on the host without using the Singularity. I am using Ubuntu 18.04, ROS melodic, OpenRAVE 0.9. I used 'Clearpath Ridgeback + UR5' model. For the simulation, everything is good. However, when I try to run on the real robot. I got the following error.
(False, False)
---------------------------------------------------------------------------
IndexError Traceback (most recent call last)
<ipython-input-2-8c9ce3e620e6> in <module>()
----> 1 demo = Demo()
2 #demo.control_robot_with_the_keyboard()
<ipython-input-1-7750c1777394> in __init__(self)
44 self.env, self.robot = ur5_factory.create_ur5_and_env(env_path="/home/host_user/catkin_leed/src/ur5controller/scripts/crihp_testbed_env.xml",
45 has_force_torque_sensor=False,
---> 46 is_simulation=False)
47 if self.env != None:
48 self.move_robot_to_test_point()
/home/host_user/catkin_leed/src/ur5controller/pythonsrc/ur5_robot/ur5_factory.pyc in create_ur5_and_env(self, test_view, is_simulation, has_ridgeback, gripper_name, has_force_torque_sensor, env_path, viewer_name, urdf_path, srdf_path)
151 # Attach controllers
152 if not is_simulation:
--> 153 self._attach_robot_controller()
154
155 if has_gripper:
/home/host_user/catkin_leed/src/ur5controller/pythonsrc/ur5_robot/ur5_factory.pyc in _attach_robot_controller(self)
190 if self._a_ros_topic_exist_with_the_name("joint_states"):
191 self.robot.attach_controller(
--> 192 name="ur5controller", dof_indices=[2, 1, 0, 4, 5, 6]
193 )
194 RaveLogInfo("UR5 controller attached successfully.")
/home/host_user/catkin_leed/src/ur5controller/pythonsrc/ur5_robot/ur5_robot.pyc in attach_controller(self, name, dof_indices)
110 # trajectories in simulation, setting the multicontroller now
111 # ensures that this won't happen.
--> 112 self.multicontroller.AttachController(controller, dof_indices, 0)
113 return True
114
IndexError: vector::_M_range_check: __n (which is 6) >= this->size() (which is 6)
My ur5robot.py
from openravepy import databases
from openravepy import IkParameterization
from openravepy import interfaces
from openravepy import RaveCreateController
from openravepy import RaveCreateMultiController
from openravepy import RaveLogInfo
from openravepy import Robot
import time
class UR5_Robot(Robot):
def __init__(self, is_in_simulation, has_gripper):
self.robot_name = "UR5"
self._OPENRAVE_GRIPPER_MAX_VALUE = 0.715584844
self._ROBOT_GRIPPER_MAX_VALUE = 255
self.is_in_simulation = is_in_simulation
self._has_gripper = has_gripper
if not self.is_in_simulation:
self.multicontroller = RaveCreateMultiController(self.GetEnv(), "")
self.SetController(self.multicontroller)
self.manipulator = self.SetActiveManipulator(self.GetManipulators()[0])
if self._has_gripper:
# Needed for "find a grasp" function (not parsed using or_urdf hence
# needs manual setting)
self.manipulator.SetChuckingDirection([1.0])
self.manipulator.SetLocalToolDirection([1.0, 0, 0])
self.ikmodel = databases.inversekinematics.InverseKinematicsModel(
self, iktype=IkParameterization.Type.Transform6D
)
if not self.ikmodel.load():
RaveLogInfo(
"The IKModel for UR5 robot is now being generated. "
"Please be patient, this will take a while "
"(sometimes up to 30 minutes)..."
)
self.ikmodel.autogenerate()
self.task_manipulation = interfaces.TaskManipulation(self)
self.base_manipulation = interfaces.BaseManipulation(self)
@property
def end_effector_transform(self):
"""End-Effector's current transform property."""
return self.manipulator.GetTransform()
def is_gripper_fully_open(self):
if not self._has_gripper:
raise Exception(
"You are trying to use a gripper function while a gripper is not available on your UR5 robot."
)
return self.GetDOFValues()[3] == 0
def is_gripper_fully_closed(self):
if not self._has_gripper:
raise Exception(
"You are trying to use a gripper function while a gripper is not available on your UR5 robot."
)
return abs(self.GetDOFValues()[3] - self._OPENRAVE_GRIPPER_MAX_VALUE) <= 0.00001
def attach_controller(self, name, dof_indices):
controller = RaveCreateController(self.GetEnv(), name)
if controller is not None:
# Attaching the multicontroller now since we know that a
# valid controller has been created. If multicontroller is
# set to the robot with no controller attached to the
# multicontroller, then OpenRAVE will fail to execute any
# trajectories in simulation, setting the multicontroller now
# ensures that this won't happen.
self.multicontroller.AttachController(controller, dof_indices, 0)
return True
return False
def set_gripper_openning(self, value):
"""
Opens/Closes the gripper by a desired value.
Args:
value: Opens/Closes the finger.
Raises:
ValueError: If the value is out of bounds [0, 255].
"""
if not self._has_gripper:
raise Exception(
"You are trying to use a gripper function while a gripper is not available on your UR5 robot."
)
if value < 0 or value > 255:
raise ValueError("Gripper value should be between 0 and 255.")
# This conversion is required because SetDesired() controller
# requires the value to be in terms of a value between 0 and
# 0.87266444 (OpenRAVE model limit values) and then is converting
# it to a value between 0-255 for the low-level gripper driver.
# To make this method more user-friendly, the value is expected
# to be between 0 and 255, then we map it down to 0 to 0.87266444
# and send it to the gripper controller.
model_value = (
self._OPENRAVE_GRIPPER_MAX_VALUE
/ self._ROBOT_GRIPPER_MAX_VALUE
* abs(value)
)
dof_values = self.GetDOFValues()
dof_values[3] = model_value
self.GetController().SetDesired(dof_values)
self.WaitForController(0)
time.sleep(2)
def _set_dof_value(self, index, value):
dof_values = self.GetDOFValues()
dof_values[index] = value
self.SetDOFValues(dof_values)
def open_gripper(self, kinbody=None, execute=True):
"""
Opens end-effector's fingers.
OpenRAVE Kinbodies can be attached to the end-effector, if you want
to release an attached kinbody after the fingers are opened, then pass
that kinbody in this function.
Args:
kinbody: Pass an OpenRAVE Kinbody to release after the fingers are
opened.
execute: By default is set to True. Pass False if you want to avoid
execution.
"""
if not self._has_gripper:
raise Exception(
"You are trying to use a gripper function while a gripper is not available on your UR5 robot."
)
if not execute:
self._set_dof_value(3, 0.0)
if kinbody is not None:
self.Release(kinbody)
return
if self.is_in_simulation:
self.task_manipulation.ReleaseFingers(target=kinbody)
self.WaitForController(0)
else:
self.set_gripper_openning(0)
if kinbody is not None:
self.Release(kinbody)
def close_gripper(self, execute=True):
"""
Closes end-effector's fingers.
Args:
execute: By default is set to True. Pass False if you want to avoid
execution.
"""
if not self._has_gripper:
raise Exception(
"You are trying to use a gripper function while a gripper is not available on your UR5 robot."
)
if not execute:
self._set_dof_value(3, self._OPENRAVE_GRIPPER_MAX_VALUE)
return
if self.is_in_simulation:
self.task_manipulation.CloseFingers()
self.WaitForController(0)
else:
self.set_gripper_openning(255)
def execute_trajectory_and_wait_for_controller(self, trajectory):
"""
Executes a trajectory and waits for the controller to finish.
Args:
trajectory: An OpenRAVE trajectory to execute.
"""
self.GetController().SetPath(trajectory)
self.WaitForController(0)
time.sleep(2)the ur5_factory.py file:
#!/usr/bin/env python
# Copyright (C) 2018 The University of Leeds.
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <http://www.gnu.org/licenses/>.
"""
Include a factory class to generate UR5 instances.
UR5 instances can be generated with different configuration. For example
the robot may have a ClearPath ridgeback, a two finger gripper etc where
in another setup the UR5 could not have the ridgeback, and could have
a three finger gripper instead.
This file define a class that let you create different robot configurations
on the fly.
"""
__author__ = "Rafael Papallas"
__authors__ = ["Rafael Papallas"]
__copyright__ = "Copyright (C) 2018, The University of Leeds"
__credits__ = ["Rafael Papallas", "Dr. Mehmet Dogar"]
__email__ = "Rafael: r.papallas@leeds.ac.uk | Mehmet: m.r.dogar@leeds.ac.uk"
__license__ = "GPLv3"
__maintainer__ = "Rafael Papallas"
__status__ = "Production"
__version__ = "0.0.1"
import rospy
from openravepy import Environment
from openravepy import RaveCreateIkSolver
from openravepy import RaveCreateModule
from openravepy import RaveInitialize
from openravepy import RaveLogInfo
from openravepy import RaveLogWarn
from ur5_robot import UR5_Robot
class UR5_Factory(object):
"""
Class for generating UR5 robot configurations and environments.
This class builds a UR5 robot by loading it from URDF files and
also adds some other implementation details that are required. This
class should be used to create UR5 instances in your OpenRAVE
programs.
"""
def __init__(self):
"""Initialise the UR5_Factory.
Initialise the class by defining available grippers and
viewers.
"""
# TODO: Add support for robotiq_three_finger
self._available_grippers = ["robotiq_two_finger"]
def create_ur5_and_env(
self,
test_view = False,
is_simulation=True,
has_ridgeback=True,
gripper_name=None,
has_force_torque_sensor=True,
env_path=None,
viewer_name="qtcoin",
urdf_path="package://ur5controller/ur5_description/urdf/",
srdf_path="package://ur5controller/ur5_description/srdf/",
):
"""
Create a UR5 and Environment instance.
This method given some specifications as arguments, will build
a UR5 robot instance accordingly.
Args:
is_simulation: Indicates whether the robot is in simulation
or not.
has_ridgeback: Indicates whether the returned robot model
has a ClearPath Ridgeback moving base
integrated with it.
gripper_name: The gripper name to be used in the robot model
among a list of available grippers. If None, no
gripper will be attached.
has_force_torque_sensor: Indicates whether the robot model
will have a Robotiq Force Torque s
ensor attached to it.
env_path: The environment XML file to load from.
viewer_name: The viewer name (e.g qtcoin, rviz etc) among
a list of available names.
urdf_path: The path of the URDF files if you moved the URDFs
to another location, if you haven't, then leave this
to default.
srdf_path: The path of the SRDF files if you moved the SRDFs
to another location, if you haven't, then leave this
to default.
Returns:
An openravepy.Environment and openravepy.Robot instances.
Raises:
ValueError: If gripper_name is invalid invalid.
"""
has_gripper = False if gripper_name is None else True
if has_gripper:
if gripper_name not in self._available_grippers:
raise ValueError(
"Gripper {} is not supported. The only "
"available gripper names are: "
"{}".format(gripper_name, ", ".join(self._available_grippers))
)
# TODO: Create URDFs that do not include the ridgeback.
if not has_ridgeback:
raise NotImplementedError(
"Robot configuration without ridgeback "
"is not yet available. Robot configuration "
"without the Clearpath Ridgeback is under "
"development at the moment (please check "
"that you also have the latest version "
"of this code)."
)
RaveInitialize(True)
self.env = self._create_environment(env_path)
self.robot = self._load_ur5_from_urdf(
gripper_name, has_ridgeback, has_force_torque_sensor, urdf_path, srdf_path
)
if test_view == False:
if not self._a_ros_topic_exist_with_the_name("joint_states"):
is_simulation = True
# Add class UR5_Robot to the robot.
self.robot.__class__ = UR5_Robot
self.robot.__init__(is_simulation, has_gripper)
print(is_simulation, has_gripper)
# Attach controllers
if not is_simulation:
self._attach_robot_controller()
if has_gripper:
self._attach_gripper_controller(gripper_name)
# Required for or_rviz to work with the robot's interactive marker.
self._set_ik_solver()
self._set_viewer(viewer_name)
return self.env, self.robot
else:
self._set_viewer(viewer_name)
return None, None
def _set_ik_solver(self):
ik_solver = RaveCreateIkSolver(self.env, self.robot.ikmodel.getikname())
self.robot.manipulator.SetIkSolver(ik_solver)
def _attach_gripper_controller(self, gripper_name):
if gripper_name == "robotiq_two_finger":
controller_name = "robotiqcontroller"
if self._a_ros_topic_exist_with_the_name("CModelRobotInput"):
self.robot.attach_controller(name=controller_name, dof_indices=[3])
RaveLogInfo("robotiq_two_finger controller attached successfully.")
else:
RaveLogWarn(
"End-effector controller not attached, "
"topics ('CModelRobotInput' or/and "
"'CModelRobotOutput') are not available."
)
def _attach_robot_controller(self):
# This is a defensive mechanism to avoid IsDone() method of the
# end-effector controller block the program execution. For further
# discussion, see this thread: https://stackoverflow.com/questions/49552755/openrave-controllerbase-is-blocking-at-the-isdone-method-and-never-returns/49552756#49552756
if self._a_ros_topic_exist_with_the_name("joint_states"):
self.robot.attach_controller(
name="ur5controller", dof_indices=[2, 1, 0, 4, 5, 6]
)
RaveLogInfo("UR5 controller attached successfully.")
else:
RaveLogWarn(
"UR5 Controller not attached to robot. Required "
"topics not being published, rolling back to "
"simulation."
)
def _a_ros_topic_exist_with_the_name(self, topic_name):
# The ROS topic name always start with '/' character.
if topic_name[0] != "/":
topic_name = "/" + topic_name
# List of all published ROS topics
topics = rospy.get_published_topics()
for topic in topics:
if topic[0] == topic_name:
return True
return False
def _get_file_name_from_specification(
self, gripper_name, has_ridgeback, has_force_torque_sensor
):
"""
Return the correct URDF file given some configuration specification.
Different robot configurations (with ridgeback and two-finger gripper
or without ridgeback and with three-finger gripper) exists with
different URDF and SRDF files. Hence this method given some
specifications will return the appropriate URDF file from the
package.
Args:
gripper_name: The gripper name to be loaded.
has_ridgeback: Whether ClearPath Ridgeback will be loaded with
the robot model.
has_force_torque_sensor: Whether Robotiq force torque sensor
will be loaded with the robot model.
Returns:
A file name to a URDF/SRDF file matching the configuration
specified by the arguments.
"""
if (
gripper_name == "robotiq_two_finger"
and has_ridgeback
and has_force_torque_sensor
):
return (
"clearpath_ridgeback__ur5__robotiq_two_finger_gripper__robotiq_fts150"
)
if gripper_name is None and has_ridgeback:
return "clearpath_ridgeback__ur5"
if (
gripper_name == "robotiq_two_finger"
and has_ridgeback
and not has_force_torque_sensor
):
return "clearpath_ridgeback__ur5__robotiq_two_finger_gripper"
if (
gripper_name == "robotiq_three_finger"
and has_ridgeback
and has_force_torque_sensor
):
return (
"clearpath_ridgeback__ur5__robotiq_three_finger_gripper__robotiq_fts150"
)
if (
gripper_name == "robotiq_three_finger"
and has_ridgeback
and not has_force_torque_sensor
):
return "clearpath_ridgeback__ur5__robotiq_three_finger_gripper"
def _load_ur5_from_urdf(
self, gripper_name, has_ridgeback, has_force_torque_sensor, urdf_path, srdf_path
):
"""
Load a UR5 robot model from URDF to the environment.
Given the appropriate configuration and valid path to URDF
and SRDF files, will load UR5 in OpenRAVE environment and will
return a robot instance back.
Args:
gripper_name: The gripper name to be loaded.
has_ridgeback: Whether ClearPath Ridgeback will be loaded with
the robot model.
has_force_torque_sensor: Whether Robotiq force torque sensor
will be loaded with the robot model.
urdf_path: The path of the URDF files.
srdf_path: The path of the SRDF files.
Returns:
A UR5 openravepy.Robot instance.
Raises:
Exception: If or_urdf module cannot be loaded, or something
went wrong while trying to load robot from a URDF file,
or the robot couldn't be loaded from file.
"""
file_name = self._get_file_name_from_specification(
gripper_name, has_ridgeback, has_force_torque_sensor
)
urdf_path = urdf_path + "{}.urdf".format(file_name)
srdf_path = srdf_path + "{}.srdf".format(file_name)
urdf_module = RaveCreateModule(self.env, "urdf")
if urdf_module is None:
raise Exception(
"Unable to load or_urdf module. Make sure you "
"have or_urdf installed in your Catkin "
"check: "
"https://github.com/personalrobotics/or_urdf"
)
ur5_name = urdf_module.SendCommand("LoadURI {} {}".format(urdf_path, srdf_path))
if ur5_name is None:
raise Exception(
"Something went wrong while trying to load "
"UR5 from file. Is the path correct?"
)
robot = self.env.GetRobot(ur5_name)
if robot is None:
raise Exception("Unable to find robot with " "name '{}'.".format(ur5_name))
return robot
def _create_environment(self, env_path):
"""
Create an Environment instance and load an XML environment to it.
Will create an openravepy.Environment instance and will try to
load an XML environment specification from file.
Args:
env_path: An XML file to load an environment.
Returns:
The openravepy.Environment instance.
Raises:
ValueError: If unable to load environment from file.
"""
env = Environment()
if env_path is not None:
if not env.Load(env_path):
raise ValueError(
"Unable to load environment " "file: '{}'".format(env_path)
)
return env
def _set_viewer(self, viewer_name):
"""
Set the viewer using the user-specified viewer_name.
Args:
viewer_name: The name of viewer to be set.
Raises:
Exception: If there was a problem setting the viewer.
"""
self.env.SetViewer(viewer_name)
if self.env.GetViewer() is None:
raise Exception(
"There was something wrong when loading "
"the {} viewer.".format(viewer_name)
)
Hello when you installed this catkin_package (ur5controller) into your catkin_ws, did you run catkin_make? The robot controller is in C++ and needs to be built before you can use it.
robopassio
commented
3 years ago Yes, I did it. The ur5controller is contained in the src directory. That's why I can control in simulation with OpenRAVE.
I also start the real robot as follows:
host_user@host_user-Latitude-5590:~/catkin_leed$ roslaunch ur_robot_driver ur5_bringup.launch
... logging to /home/host_user/.ros/log/f93ff1ee-5977-11eb-8fc4-0c96e68fd9a7/roslaunch-host_user-Latitude-5590-19254.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
xacro: in-order processing became default in ROS Melodic. You can drop the option.
started roslaunch server http://host_user-Latitude-5590:35385/
SUMMARY
========
PARAMETERS
* /controller_stopper/consistent_controllers: ['joint_state_con...
* /force_torque_sensor_controller/publish_rate: 125
* /force_torque_sensor_controller/type: force_torque_sens...
* /hardware_control_loop/loop_hz: 125
* /joint_group_vel_controller/joints: ['shoulder_pan_jo...
* /joint_group_vel_controller/type: velocity_controll...
* /joint_state_controller/publish_rate: 125
* /joint_state_controller/type: joint_state_contr...
* /pos_joint_traj_controller/action_monitor_rate: 20
* /pos_joint_traj_controller/constraints/elbow_joint/goal: 0.1
* /pos_joint_traj_controller/constraints/elbow_joint/trajectory: 0.2
* /pos_joint_traj_controller/constraints/goal_time: 0.6
* /pos_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
* /pos_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.2
* /pos_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
* /pos_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.2
* /pos_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
* /pos_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
* /pos_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.2
* /pos_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
* /pos_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.2
* /pos_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
* /pos_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.2
* /pos_joint_traj_controller/joints: ['shoulder_pan_jo...
* /pos_joint_traj_controller/state_publish_rate: 125
* /pos_joint_traj_controller/stop_trajectory_duration: 0.5
* /pos_joint_traj_controller/type: position_controll...
* /robot_description: <?xml version="1....
* /robot_status_controller/handle_name: industrial_robot_...
* /robot_status_controller/publish_rate: 10
* /robot_status_controller/type: industrial_robot_...
* /rosdistro: melodic
* /rosversion: 1.14.10
* /scaled_pos_joint_traj_controller/action_monitor_rate: 20
* /scaled_pos_joint_traj_controller/constraints/elbow_joint/goal: 0.1
* /scaled_pos_joint_traj_controller/constraints/elbow_joint/trajectory: 0.2
* /scaled_pos_joint_traj_controller/constraints/goal_time: 0.6
* /scaled_pos_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
* /scaled_pos_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.2
* /scaled_pos_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
* /scaled_pos_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.2
* /scaled_pos_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
* /scaled_pos_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
* /scaled_pos_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.2
* /scaled_pos_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
* /scaled_pos_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.2
* /scaled_pos_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
* /scaled_pos_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.2
* /scaled_pos_joint_traj_controller/joints: ['shoulder_pan_jo...
* /scaled_pos_joint_traj_controller/state_publish_rate: 125
* /scaled_pos_joint_traj_controller/stop_trajectory_duration: 0.5
* /scaled_pos_joint_traj_controller/type: position_controll...
* /scaled_vel_joint_traj_controller/action_monitor_rate: 20
* /scaled_vel_joint_traj_controller/constraints/elbow_joint/goal: 0.1
* /scaled_vel_joint_traj_controller/constraints/elbow_joint/trajectory: 0.1
* /scaled_vel_joint_traj_controller/constraints/goal_time: 0.6
* /scaled_vel_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
* /scaled_vel_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.1
* /scaled_vel_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
* /scaled_vel_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.1
* /scaled_vel_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
* /scaled_vel_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
* /scaled_vel_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.1
* /scaled_vel_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
* /scaled_vel_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.1
* /scaled_vel_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
* /scaled_vel_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.1
* /scaled_vel_joint_traj_controller/gains/elbow_joint/d: 0.1
* /scaled_vel_joint_traj_controller/gains/elbow_joint/i: 0.05
* /scaled_vel_joint_traj_controller/gains/elbow_joint/i_clamp: 1
* /scaled_vel_joint_traj_controller/gains/elbow_joint/p: 5.0
* /scaled_vel_joint_traj_controller/gains/shoulder_lift_joint/d: 0.1
* /scaled_vel_joint_traj_controller/gains/shoulder_lift_joint/i: 0.05
* /scaled_vel_joint_traj_controller/gains/shoulder_lift_joint/i_clamp: 1
* /scaled_vel_joint_traj_controller/gains/shoulder_lift_joint/p: 5.0
* /scaled_vel_joint_traj_controller/gains/shoulder_pan_joint/d: 0.1
* /scaled_vel_joint_traj_controller/gains/shoulder_pan_joint/i: 0.05
* /scaled_vel_joint_traj_controller/gains/shoulder_pan_joint/i_clamp: 1
* /scaled_vel_joint_traj_controller/gains/shoulder_pan_joint/p: 5.0
* /scaled_vel_joint_traj_controller/gains/wrist_1_joint/d: 0.1
* /scaled_vel_joint_traj_controller/gains/wrist_1_joint/i: 0.05
* /scaled_vel_joint_traj_controller/gains/wrist_1_joint/i_clamp: 1
* /scaled_vel_joint_traj_controller/gains/wrist_1_joint/p: 5.0
* /scaled_vel_joint_traj_controller/gains/wrist_2_joint/d: 0.1
* /scaled_vel_joint_traj_controller/gains/wrist_2_joint/i: 0.05
* /scaled_vel_joint_traj_controller/gains/wrist_2_joint/i_clamp: 1
* /scaled_vel_joint_traj_controller/gains/wrist_2_joint/p: 5.0
* /scaled_vel_joint_traj_controller/gains/wrist_3_joint/d: 0.1
* /scaled_vel_joint_traj_controller/gains/wrist_3_joint/i: 0.05
* /scaled_vel_joint_traj_controller/gains/wrist_3_joint/i_clamp: 1
* /scaled_vel_joint_traj_controller/gains/wrist_3_joint/p: 5.0
* /scaled_vel_joint_traj_controller/joints: ['shoulder_pan_jo...
* /scaled_vel_joint_traj_controller/state_publish_rate: 125
* /scaled_vel_joint_traj_controller/stop_trajectory_duration: 0.5
* /scaled_vel_joint_traj_controller/type: velocity_controll...
* /scaled_vel_joint_traj_controller/velocity_ff/elbow_joint: 1.0
* /scaled_vel_joint_traj_controller/velocity_ff/shoulder_lift_joint: 1.0
* /scaled_vel_joint_traj_controller/velocity_ff/shoulder_pan_joint: 1.0
* /scaled_vel_joint_traj_controller/velocity_ff/wrist_1_joint: 1.0
* /scaled_vel_joint_traj_controller/velocity_ff/wrist_2_joint: 1.0
* /scaled_vel_joint_traj_controller/velocity_ff/wrist_3_joint: 1.0
* /speed_scaling_state_controller/publish_rate: 125
* /speed_scaling_state_controller/type: ur_controllers/Sp...
* /ur_hardware_interface/headless_mode: False
* /ur_hardware_interface/input_recipe_file: /home/host_user/cat...
* /ur_hardware_interface/joints: ['shoulder_pan_jo...
* /ur_hardware_interface/kinematics/forearm/pitch: 0
* /ur_hardware_interface/kinematics/forearm/roll: 0
* /ur_hardware_interface/kinematics/forearm/x: -0.425
* /ur_hardware_interface/kinematics/forearm/y: 0
* /ur_hardware_interface/kinematics/forearm/yaw: 0
* /ur_hardware_interface/kinematics/forearm/z: 0
* /ur_hardware_interface/kinematics/hash: calib_20954911754...
* /ur_hardware_interface/kinematics/shoulder/pitch: 0
* /ur_hardware_interface/kinematics/shoulder/roll: 0
* /ur_hardware_interface/kinematics/shoulder/x: 0
* /ur_hardware_interface/kinematics/shoulder/y: 0
* /ur_hardware_interface/kinematics/shoulder/yaw: 0
* /ur_hardware_interface/kinematics/shoulder/z: 0.089159
* /ur_hardware_interface/kinematics/upper_arm/pitch: 0
* /ur_hardware_interface/kinematics/upper_arm/roll: 1.570796327
* /ur_hardware_interface/kinematics/upper_arm/x: 0
* /ur_hardware_interface/kinematics/upper_arm/y: 0
* /ur_hardware_interface/kinematics/upper_arm/yaw: 0
* /ur_hardware_interface/kinematics/upper_arm/z: 0
* /ur_hardware_interface/kinematics/wrist_1/pitch: 0
* /ur_hardware_interface/kinematics/wrist_1/roll: 0
* /ur_hardware_interface/kinematics/wrist_1/x: -0.39225
* /ur_hardware_interface/kinematics/wrist_1/y: 0
* /ur_hardware_interface/kinematics/wrist_1/yaw: 0
* /ur_hardware_interface/kinematics/wrist_1/z: 0.10915
* /ur_hardware_interface/kinematics/wrist_2/pitch: 0
* /ur_hardware_interface/kinematics/wrist_2/roll: 1.570796327
* /ur_hardware_interface/kinematics/wrist_2/x: 0
* /ur_hardware_interface/kinematics/wrist_2/y: -0.09465
* /ur_hardware_interface/kinematics/wrist_2/yaw: 0
* /ur_hardware_interface/kinematics/wrist_2/z: -1.9413039509e-11
* /ur_hardware_interface/kinematics/wrist_3/pitch: 3.14159265359
* /ur_hardware_interface/kinematics/wrist_3/roll: 1.57079632659
* /ur_hardware_interface/kinematics/wrist_3/x: 0
* /ur_hardware_interface/kinematics/wrist_3/y: 0.0823
* /ur_hardware_interface/kinematics/wrist_3/yaw: 3.14159265359
* /ur_hardware_interface/kinematics/wrist_3/z: -1.68800121668e-11
* /ur_hardware_interface/output_recipe_file: /home/host_user/cat...
* /ur_hardware_interface/reverse_port: 50001
* /ur_hardware_interface/robot_ip: 192.168.0.143
* /ur_hardware_interface/script_file: /home/host_user/cat...
* /ur_hardware_interface/script_sender_port: 50002
* /ur_hardware_interface/tf_prefix:
* /ur_hardware_interface/tool_baud_rate: 115200
* /ur_hardware_interface/tool_parity: 0
* /ur_hardware_interface/tool_rx_idle_chars: 1.5
* /ur_hardware_interface/tool_stop_bits: 1
* /ur_hardware_interface/tool_tx_idle_chars: 3.5
* /ur_hardware_interface/tool_voltage: 0
* /ur_hardware_interface/use_tool_communication: False
* /vel_joint_traj_controller/action_monitor_rate: 20
* /vel_joint_traj_controller/constraints/elbow_joint/goal: 0.1
* /vel_joint_traj_controller/constraints/elbow_joint/trajectory: 0.1
* /vel_joint_traj_controller/constraints/goal_time: 0.6
* /vel_joint_traj_controller/constraints/shoulder_lift_joint/goal: 0.1
* /vel_joint_traj_controller/constraints/shoulder_lift_joint/trajectory: 0.1
* /vel_joint_traj_controller/constraints/shoulder_pan_joint/goal: 0.1
* /vel_joint_traj_controller/constraints/shoulder_pan_joint/trajectory: 0.1
* /vel_joint_traj_controller/constraints/stopped_velocity_tolerance: 0.05
* /vel_joint_traj_controller/constraints/wrist_1_joint/goal: 0.1
* /vel_joint_traj_controller/constraints/wrist_1_joint/trajectory: 0.1
* /vel_joint_traj_controller/constraints/wrist_2_joint/goal: 0.1
* /vel_joint_traj_controller/constraints/wrist_2_joint/trajectory: 0.1
* /vel_joint_traj_controller/constraints/wrist_3_joint/goal: 0.1
* /vel_joint_traj_controller/constraints/wrist_3_joint/trajectory: 0.1
* /vel_joint_traj_controller/gains/elbow_joint/d: 0.1
* /vel_joint_traj_controller/gains/elbow_joint/i: 0.05
* /vel_joint_traj_controller/gains/elbow_joint/i_clamp: 1
* /vel_joint_traj_controller/gains/elbow_joint/p: 5.0
* /vel_joint_traj_controller/gains/shoulder_lift_joint/d: 0.1
* /vel_joint_traj_controller/gains/shoulder_lift_joint/i: 0.05
* /vel_joint_traj_controller/gains/shoulder_lift_joint/i_clamp: 1
* /vel_joint_traj_controller/gains/shoulder_lift_joint/p: 5.0
* /vel_joint_traj_controller/gains/shoulder_pan_joint/d: 0.1
* /vel_joint_traj_controller/gains/shoulder_pan_joint/i: 0.05
* /vel_joint_traj_controller/gains/shoulder_pan_joint/i_clamp: 1
* /vel_joint_traj_controller/gains/shoulder_pan_joint/p: 5.0
* /vel_joint_traj_controller/gains/wrist_1_joint/d: 0.1
* /vel_joint_traj_controller/gains/wrist_1_joint/i: 0.05
* /vel_joint_traj_controller/gains/wrist_1_joint/i_clamp: 1
* /vel_joint_traj_controller/gains/wrist_1_joint/p: 5.0
* /vel_joint_traj_controller/gains/wrist_2_joint/d: 0.1
* /vel_joint_traj_controller/gains/wrist_2_joint/i: 0.05
* /vel_joint_traj_controller/gains/wrist_2_joint/i_clamp: 1
* /vel_joint_traj_controller/gains/wrist_2_joint/p: 5.0
* /vel_joint_traj_controller/gains/wrist_3_joint/d: 0.1
* /vel_joint_traj_controller/gains/wrist_3_joint/i: 0.05
* /vel_joint_traj_controller/gains/wrist_3_joint/i_clamp: 1
* /vel_joint_traj_controller/gains/wrist_3_joint/p: 5.0
* /vel_joint_traj_controller/joints: ['shoulder_pan_jo...
* /vel_joint_traj_controller/state_publish_rate: 125
* /vel_joint_traj_controller/stop_trajectory_duration: 0.5
* /vel_joint_traj_controller/type: velocity_controll...
* /vel_joint_traj_controller/velocity_ff/elbow_joint: 1.0
* /vel_joint_traj_controller/velocity_ff/shoulder_lift_joint: 1.0
* /vel_joint_traj_controller/velocity_ff/shoulder_pan_joint: 1.0
* /vel_joint_traj_controller/velocity_ff/wrist_1_joint: 1.0
* /vel_joint_traj_controller/velocity_ff/wrist_2_joint: 1.0
* /vel_joint_traj_controller/velocity_ff/wrist_3_joint: 1.0
NODES
/
controller_stopper (controller_stopper/node)
robot_state_publisher (robot_state_publisher/robot_state_publisher)
ros_control_controller_spawner (controller_manager/spawner)
ros_control_stopped_spawner (controller_manager/spawner)
ur_hardware_interface (ur_robot_driver/ur_robot_driver_node)
/ur_hardware_interface/
ur_robot_state_helper (ur_robot_driver/robot_state_helper)
auto-starting new master
process[master]: started with pid [19267]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to f93ff1ee-5977-11eb-8fc4-0c96e68fd9a7
process[rosout-1]: started with pid [19278]
started core service [/rosout]
process[robot_state_publisher-2]: started with pid [19285]
process[ur_hardware_interface-3]: started with pid [19286]
process[ros_control_controller_spawner-4]: started with pid [19287]
process[ros_control_stopped_spawner-5]: started with pid [19294]
[ INFO] [1610965739.364111657]: Initializing dashboard client
process[controller_stopper-6]: started with pid [19302]
process[ur_hardware_interface/ur_robot_state_helper-7]: started with pid [19303]
[ INFO] [1610965739.377196651]: Initializing urdriver
[ INFO] [1610965739.377686645]: Waiting for controller manager service to come up on /controller_manager/switch_controller
Warning: No realtime capabilities found. Consider using a realtime system for better performance
at line 396 in /tmp/binarydeb/ros-melodic-ur-client-library-0.1.1/include/ur_client_library/comm/pipeline.h
[ INFO] [1610965739.378492726]: waitForService: Service [/controller_manager/switch_controller] has not been advertised, waiting...
[INFO] [1610965739.647784]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1610965739.657014]: Controller Spawner: Waiting for service controller_manager/load_controller
Error: The calibration parameters of the connected robot don't match the ones from the given kinematics config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the ur_calibration tool to extract the correct calibration from the robot and pass that into the description. See [https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] for details.
at line 45 in /tmp/binarydeb/ros-melodic-ur-client-library-0.1.1/src/ur/calibration_checker.cpp
Warning: No realtime capabilities found. Consider using a realtime system for better performance
at line 396 in /tmp/binarydeb/ros-melodic-ur-client-library-0.1.1/include/ur_client_library/comm/pipeline.h
Warning: No realtime capabilities found. Consider using a realtime system for better performance
at line 396 in /tmp/binarydeb/ros-melodic-ur-client-library-0.1.1/include/ur_client_library/comm/pipeline.h
[ INFO] [1610965742.014729972]: Loaded ur_robot_driver hardware_interface
[ INFO] [1610965742.049391483]: waitForService: Service [/controller_manager/switch_controller] is now available.
[ INFO] [1610965742.049467293]: Service available.
[ INFO] [1610965742.049493658]: Waiting for controller list service to come up on /controller_manager/list_controllers
[ INFO] [1610965742.049946628]: Service available.
[INFO] [1610965742.081738]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1610965742.083712]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1610965742.085559]: Loading controller: joint_state_controller
[INFO] [1610965742.087854]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1610965742.090383]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1610965742.092672]: Loading controller: pos_joint_traj_controller
[INFO] [1610965742.092821]: Loading controller: scaled_pos_joint_traj_controller
[INFO] [1610965742.120832]: Loading controller: speed_scaling_state_controller
[INFO] [1610965742.140732]: Loading controller: joint_group_vel_controller
[INFO] [1610965742.142706]: Loading controller: force_torque_sensor_controller
[INFO] [1610965742.148712]: Controller Spawner: Loaded controllers: pos_joint_traj_controller, joint_group_vel_controller
[INFO] [1610965742.150644]: Controller Spawner: Loaded controllers: joint_state_controller, scaled_pos_joint_traj_controller, speed_scaling_state_controller, force_torque_sensor_controller
[INFO] [1610965742.154857]: Started controllers: joint_state_controller, scaled_pos_joint_traj_controller, speed_scaling_state_controller, force_torque_sensor_controller
[ INFO] [1610965742.261766215]: Robot's safety mode is now NORMAL
[ INFO] [1610965742.265001237]: Robot mode is now RUNNING
[ INFO] [1610965800.095249074]: Robot requested program
[ INFO] [1610965800.095488459]: Sent program to robot
[ INFO] [1610965800.165750894]: Robot ready to receive control commands.Everythings are ready to work.
robopassio
commented
3 years ago @lakshmip001 can you run the source with a real robot? @rpapallas, do you have any suggestions for my case? Btw, can you please explain about the DOF indices in:
self.robot.attach_controller(
name="ur5controller", dof_indices=[2, 1, 0, 4, 5, 6]
)How do we know the order is 2, 1, 0, 4, 5, 6?
rpapallas
commented
3 years ago Hi,
I am looking this for you. I will get back to you soon.
The DOF Indices are mapped to the model DOF. When the model and IK solutions were generated by OpenRAVE there were given in that order if I remember right.
rpapallas
commented
3 years ago Hello,
I see that you are running the roslaunch ur_robot_driver ur5_bringup.launch without passing an IP address of the robot. Are you sure that the command is actually valid? I remember we used to run it this way:
roslaunch ur_robot_driver ur5_bringup.launch robot_ip:=IP_OF_THE_ROBOT.
To ensure that the driver is actually running and the robot is connected, try to echo the following topic and ensure it's echoing the joint angles of the robot in real-time:
rostopic echo /joint_states
robopassio
commented
3 years ago Thanks for your attention. I already set the default IP inside the .launch file. Therefore, I don't need to pass the IP argument.
Hi,
I am looking this for you. I will get back to you soon.
The DOF Indices are mapped to the model DOF. When the model and IK solutions were generated by OpenRAVE there were given in that order if I remember right.
Can you share a link about this? I still don't know where that order is defined. As the previous file,
/home/host_user/catkin_leed/src/ur5controller/pythonsrc/ur5_robot/ur5_factory.pyc in _attach_robot_controller(self)
190 if self._a_ros_topic_exist_with_the_name("joint_states"):
191 self.robot.attach_controller(
--> 192 name="ur5controller", dof_indices=[2, 1, 0, 4, 5, 6]
193 )
194 RaveLogInfo("UR5 controller attached successfully.")
/home/host_user/catkin_leed/src/ur5controller/pythonsrc/ur5_robot/ur5_robot.pyc in attach_controller(self, name, dof_indices)
110 # trajectories in simulation, setting the multicontroller now
111 # ensures that this won't happen.
--> 112 self.multicontroller.AttachController(controller, dof_indices, 0)
113 return True
114
IndexError: vector::_M_range_check: __n (which is 6) >= this->size() (which is 6)I think the problem is at IndexError: vector::_M_range_check: __n (which is 6) >= this->size() (which is 6).
Here, the condition 6 >=6 need to be correct. However, the source indicates an error. Is there any missing at this point (argument type, ...)? Can you recheck it plz?
rpapallas
commented
3 years ago The DOF Indices can be found by printing the active DOF indices from a robot object in openrave.
Try to get into simulation mode again, and using the robot object call .GetActiveDOFIndices(). See here.
Try to print out the active DOFs and let me know.
I am sorry that I am slow with this. I am quite busy lately and I don't have access to a computer with OpenRAVE and the UR5 robot. I will try to help.
The line self.multicontroller.AttachController(controller, dof_indices, 0) is implemented by OpenRAVE and my controller makes use of it. So it seems that the code is failing when attaching the controller. Let's see the output of the active DOFs when the robot is in simulation and working and take it from there.
robopassio
commented
3 years ago Thanks for your time. From the https://github.com/roboticsleeds/ur5controller/blob/master/scripts/control_ur5.py : After loading the robot and environment successfully, I try:
demo.robot.GetActiveDOFIndices()However, it the result is:
array([], dtype=int32)While I can get the result of the IKfast and control the robot without any problem.
rpapallas
commented
3 years ago Can you please run in a terminal: openrave --listplugins and paste here the output?
Also can you include the script you are using to make this demo and connect to the real robot? You have posted so far the ur5_robot and ur5_factory included in this repo, but can I see the script you are using to create this demo?
Thanks, Rafael
robopassio
commented
3 years ago Please take a look:
host_user@host_user-Latitude-5590:/usr/local$ openrave --listplugins
Number of plugins: 19
planner: 16
BasicRRT - /usr/local/lib/openrave0.9-plugins/librplanners.so
BiRRT - /usr/local/lib/openrave0.9-plugins/librplanners.so
ConstraintParabolicSmoother - /usr/local/lib/openrave0.9-plugins/librplanners.so
CubicTrajectoryRetimer - /usr/local/lib/openrave0.9-plugins/librplanners.so
ExplorationRRT - /usr/local/lib/openrave0.9-plugins/librplanners.so
GraspGradient - /usr/local/lib/openrave0.9-plugins/librplanners.so
Grasper - /usr/local/lib/openrave0.9-plugins/libgrasper.so
LinearSmoother - /usr/local/lib/openrave0.9-plugins/librplanners.so
LinearTrajectoryRetimer - /usr/local/lib/openrave0.9-plugins/librplanners.so
ParabolicSmoother - /usr/local/lib/openrave0.9-plugins/librplanners.so
ParabolicSmoother2 - /usr/local/lib/openrave0.9-plugins/librplanners.so
ParabolicTrajectoryRetimer - /usr/local/lib/openrave0.9-plugins/librplanners.so
ParabolicTrajectoryRetimer2 - /usr/local/lib/openrave0.9-plugins/librplanners.so
RAStar - /usr/local/lib/openrave0.9-plugins/librplanners.so
WorkspaceTrajectoryTracker - /usr/local/lib/openrave0.9-plugins/librplanners.so
shortcut_linear - /usr/local/lib/openrave0.9-plugins/librplanners.so
robot: 3
CollisionMapRobot - /usr/local/lib/openrave0.9-plugins/libbaserobots.so
Conveyor - /usr/local/lib/openrave0.9-plugins/libbaserobots.so
GenericRobot - __internal__
sensorsystem: 0
controller: 7
GenericMultiController - __internal__
IdealController - /usr/local/lib/openrave0.9-plugins/libbasecontrollers.so
IdealVelocityController - /usr/local/lib/openrave0.9-plugins/libbasecontrollers.so
RedirectController - /usr/local/lib/openrave0.9-plugins/libbasecontrollers.so
RobotiqController - /home/host_user/catkin_leed/devel/share/openrave-0.9/plugins/ur5controller_plugin.so
Ur5Controller - /home/host_user/catkin_leed/devel/share/openrave-0.9/plugins/ur5controller_plugin.so
odevelocity - /usr/local/lib/openrave0.9-plugins/liboderave.so
module: 11
BaseManipulation - /usr/local/lib/openrave0.9-plugins/librmanipulation.so
DualManipulation - /usr/local/lib/openrave0.9-plugins/libdualmanipulation.so
Grasper - /usr/local/lib/openrave0.9-plugins/libgrasper.so
IvModelLoader - /usr/local/lib/openrave0.9-plugins/libqtcoinrave.so
TaskCaging - /usr/local/lib/openrave0.9-plugins/librmanipulation.so
TaskManipulation - /usr/local/lib/openrave0.9-plugins/librmanipulation.so
URDF - /home/host_user/catkin_leed/devel/lib/openrave-0.9/or_urdf_plugin.so
ViewerRecorder - /usr/local/lib/openrave0.9-plugins/liblogging.so
VisualFeedback - /usr/local/lib/openrave0.9-plugins/librmanipulation.so
ikfast - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
textserver - /usr/local/lib/openrave0.9-plugins/libtextserver.so
iksolver: 13
ikfast - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_katana5d - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_katana5d_trans - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_pr2_head - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_pr2_head_torso - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_pr2_leftarm - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_pr2_leftarm_torso - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_pr2_rightarm - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_pr2_rightarm_torso - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
ikfast_schunk_lwa3 - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
pa10ikfast - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
pumaikfast - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
wam7ikfast - /usr/local/lib/openrave0.9-plugins/libikfastsolvers.so
kinbody: 0
physicsengine: 2
GenericPhysicsEngine - __internal__
ode - /usr/local/lib/openrave0.9-plugins/liboderave.so
sensor: 7
BaseCamera - /usr/local/lib/openrave0.9-plugins/libbasesensors.so
BaseFlashLidar3D - /usr/local/lib/openrave0.9-plugins/libbasesensors.so
BaseLaser2D - /usr/local/lib/openrave0.9-plugins/libbasesensors.so
BaseSpinningLaser2D - /usr/local/lib/openrave0.9-plugins/libbasesensors.so
base_laser2d - /usr/local/lib/openrave0.9-plugins/libbasesensors.so
base_laser3d - /usr/local/lib/openrave0.9-plugins/libbasesensors.so
base_pinhole_camera - /usr/local/lib/openrave0.9-plugins/libbasesensors.so
collisionchecker: 5
CacheChecker - /usr/local/lib/openrave0.9-plugins/libconfigurationcache.so
GenericCollisionChecker - __internal__
fcl_ - /usr/local/lib/openrave0.9-plugins/libfclrave.so
ode - /usr/local/lib/openrave0.9-plugins/liboderave.so
pqp - /usr/local/lib/openrave0.9-plugins/libpqprave.so
trajectory: 1
GenericTrajectory - __internal__
viewer: 2
QtCameraViewer - /usr/local/lib/openrave0.9-plugins/libqtcoinrave.so
QtCoin - /usr/local/lib/openrave0.9-plugins/libqtcoinrave.so
spacesampler: 5
BodyConfiguration - /usr/local/lib/openrave0.9-plugins/libbasesamplers.so
ConfigurationJitterer - /usr/local/lib/openrave0.9-plugins/libconfigurationcache.so
Halton - /usr/local/lib/openrave0.9-plugins/libbasesamplers.so
MT19937 - /usr/local/lib/openrave0.9-plugins/libbasesamplers.so
RobotConfiguration - /usr/local/lib/openrave0.9-plugins/libbasesamplers.soFor the demo code, I used this script:
https://github.com/roboticsleeds/ur5controller/blob/master/scripts/control_ur5.py
Thanks.
robopassio
commented
3 years ago The results of rostopic echo /joint_states
---
header:
seq: 16543
stamp:
secs: 1611123583
nsecs: 940227324
frame_id: ''
name: [elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint,
wrist_3_joint]
position: [-2.2398386001586914, -0.8661115926555176, -1.4089649359332483, -1.6119162044920863, 0.1350560188293457, 0.2465200424194336]
velocity: [-0.0, 0.0, 0.0, -0.0, 0.0, -0.0]
effort: [1.9754064083099365, -0.6682823896408081, -0.03312399238348007, 0.21311014890670776, -0.0854506865143776, 0.021550998091697693]
I am looking at this in more detail.
Can you please run the Python script that causes the error using the pdb?
python -m pdb script.py
and then once the script crashed, run bt to get the backtrace and paste it here to see where exactly the error is.
rpapallas
commented
3 years ago Can you also try the following code and let me know if it creates the same problem?
import IPython
env = Environment()
env.Load('test_env.xml')
env.SetViewer('qtcoin')
urdf_path = "package://ur5controller/ur5_description/clearpath_ridgeback__ur5.urdf"
srdf_path = "package://ur5controller/ur5_description/clearpath_ridgeback__ur5.srdf"
module = RaveCreateModule(env, 'urdf')
with env:
name = module.SendCommand('LoadURI {} {}'.format(urdf_path, srdf_path))
robot = env.GetRobot(name)
env.Add(robot, True)
multicontroller = RaveCreateMultiController(env, "")
robot.SetController(multicontroller)
robot_controller = RaveCreateController(env,'ur5controller')
multicontroller.AttachController(robot_controller, [2, 1, 0, 4, 5, 6], 0)
IPython.embed()@robopassio Did you have any luck with this?
robopassio
commented
3 years ago My last test got no luck, however, I will send you feedback later.
I just have one more question, where is the indice #3 in this command
multicontroller.AttachController(robot_controller, [2, 1, 0, 4, 5, 6], 0)
rpapallas
commented
3 years ago That was reserved for the end-effector joint.
rpapallas
commented
3 years ago Did you have any luck with this issue? @robopassio
I am trying to use only universal robot without a gripper but I face following error when trying to attach_controller:
please suggest me how tosolve this error and proceed.