def moveTo(self, x, y, theta, frame, _async=False):
"""
Move the robot in frame world or robot (FRAME_WORLD=1, FRAME_ROBOT=2).
This method can be called synchonously or asynchronously. In the
asynchronous mode, the function can be called when it's already
launched, this will update the goal of the motion.
Parameters:
x - position of the goal on the x axis, in meters
y - position of the goal on the y axis, in meters
theta - orientation of the goal around the z axis, in radians
frame - The frame in which the goal is expressed: FRAME_WORLD = 1,
FRAME_ROBOT = 2
_async - The method is launched in async mode if True, in synch
mode if False (False by default)
"""
self._setGoal(x, y, theta, frame)
if self.module_process.isAlive():
if _async is False:
raise pybullet.error("Already a moveTo asynchronous. Can't "
"launch moveTo synchronous")
self._initProcess()
elif _async:
self.module_process = threading.Thread(target=self._moveToProcess)
self.module_process.start()
else:
self._moveToProcess()
def getValues(self, fsr_names):
"""
Returns all of the FSR weight values for the FSRs corresponding to the
passed names. If the list of passed names is empty, the method will
return an empty list. If one of the required FSRs does not exist, the
method will raise a pybullet error
Parameters:
fsr_names - List containing the FSR names (for instance
NaoFsr.LFOOT)
Returns:
fsr_values - The measured values for the corresponding FSRs, as a
List
"""
values = list()
try:
for name in fsr_names:
values.append(self.fsr_dict[name].getValue())
return values
except KeyError:
raise pybullet.error("The required Fsr does not exist")
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Loads the robot into a simulation, loads the joints and the links
descriptions. The joints are set to 0 rad.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
Returns:
boolean - True if the method ran correctly, False otherwise
"""
try:
self.physics_client = physicsClientId
self.robot_model = pybullet.loadURDF(
self.description_file,
translation,
quaternion,
useFixedBase=False,
globalScaling=1.0,
physicsClientId=self.physics_client,
flags=pybullet.URDF_USE_SELF_COLLISION |
pybullet.URDF_USE_MATERIAL_COLORS_FROM_MTL)
except pybullet.error as e:
raise pybullet.error("Cannot load robot model: " + str(e))
for i in range(pybullet.getNumJoints(
self.robot_model,
physicsClientId=self.physics_client)):
if IS_VERSION_PYTHON_3:
# PYTHON 3 version needs a conversion bytes to str
joint_info = pybullet.getJointInfo(
self.robot_model,
i,
physicsClientId=self.physics_client)
self.link_dict[joint_info[12].decode('utf-8')] =\
Link(joint_info)
if joint_info[2] == pybullet.JOINT_PRISMATIC or\
joint_info[2] == pybullet.JOINT_REVOLUTE:
self.joint_dict[joint_info[1].decode('utf-8')] =\
Joint(joint_info)
else:
# PYTHON 2 Version
joint_info = pybullet.getJointInfo(
self.robot_model,
i,
physicsClientId=self.physics_client)
self.link_dict[joint_info[12]] = Link(joint_info)
if joint_info[2] == pybullet.JOINT_PRISMATIC or\
joint_info[2] == pybullet.JOINT_REVOLUTE:
self.joint_dict[joint_info[1]] = Joint(joint_info)
def _moveToCallback(self, msg):
"""
INTERNAL METHOD, callback triggered when a message is received on the
'/move_base_simple/goal' topic. It allows to move the robot's base
Parameters:
msg - a ROS message containing a pose stamped with a speed, or a
simple pose stamped (depending on which version of the naoqi_driver
is used, the "official" one from ros-naoqi or the "non official"
softbankrobotics-research fork). The type of the message is the
following: geometry_msgs::PoseStamped for the "official",
naoqi_bridge_msgs::PoseStampedWithSpeed for the "non-official".
An alias is given to the message type: MovetoPose
"""
if OFFICIAL_DRIVER:
pose = msg.pose
frame = 0
frame_id = msg.header.frame_id
speed = None
else:
pose = msg.pose_stamped.pose
frame = msg.referenceFrame
frame_id = msg.pose_stamped.header.frame_id
speed = msg.speed_percentage *\
PepperBaseController.MAX_LINEAR_VELOCITY +\
PepperBaseController.MIN_LINEAR_VELOCITY
try:
assert frame not in [
PepperVirtual.FRAME_ROBOT, PepperVirtual.FRAME_WORLD
]
if frame_id == "odom":
frame = PepperVirtual.FRAME_WORLD
elif frame_id == "base_footprint":
frame = PepperVirtual.FRAME_ROBOT
else:
raise pybullet.error(
"Incorrect reference frame for move_base_simple, please "
"modify the content of your message")
except AssertionError:
pass
x = pose.position.x
y = pose.position.y
theta = pybullet.getEulerFromQuaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])[-1]
self.virtual_robot.moveTo(x,
y,
theta,
frame=frame,
speed=speed,
_async=True)
def close(self):
r""" pybullet crashes if the env is already closed. We want to ignore this. """
try:
# The regular close method seems to have been decrecated.
self.env.env._close()
except pybullet.error as err:
if not str(err).__contains__(
'Not connected to physics server'):
raise pybullet.error(err)
def step(self, action):
try:
return super(BulletWrapper, self).step(action)
except pybullet.error as err:
if not str(err).__contains__(
'Not connected to physics server'):
raise pybullet.error(err)
warn('Gyn Env engine failed, attempting to reinitialize.')
self.env = gym.make(self.env.spec.id)
self._env_init()
return super(BulletWrapper, self).step(action)
def __init__(self):
"""
Constructor
"""
if MISSING_IMPORT is not None:
raise pybullet.error(MISSING_IMPORT)
self.spin_thread = None
self._wrapper_termination = False
self.image_bridge = CvBridge()
self.roslauncher = None
self.transform_broadcaster = tf2_ros.TransformBroadcaster()
atexit.register(self.stopWrapper)
def __init__(self):
"""
Constructor
"""
if MISSING_IMPORT is not None:
raise pybullet.error(MISSING_IMPORT)
self.spin_thread = None
self.image_bridge = CvBridge()
self.front_info_msg = dict()
self.bottom_info_msg = dict()
self.depth_info_msg = dict()
self._loadCameraInfos()
self.transform_broadcaster = tf2_ros.TransformBroadcaster()
def setAngles(self, joint_names, joint_values, percentage_speed):
"""
Overloads @setAngles from the @RobotVirtual class. Handles the finger
mimic behaviour.
Parameters:
joint_names - List of string (or string if only one joint)
containing the name of the joints to be controlled
joint_values - List of values (or value if only one joint)
corresponding to the angles in radians to be applied
percentage_speed - Percentage (or percentages) of the max speed to
be used for the movement
"""
try:
if type(joint_names) is str:
assert type(joint_values) is int or type(joint_values) is float
names = [joint_names]
values = [joint_values]
else:
assert type(joint_names) is type(joint_values) is list
names = list(joint_names)
values = list(joint_values)
if isinstance(percentage_speed, list):
speeds = list(percentage_speed)
else:
speeds = [percentage_speed]*len(names)
except AssertionError:
raise pybullet.error("Error in the parameters given to the\
setAngles method")
for hand in ["RHand", "LHand"]:
for i in range(names.count(hand)):
index = names.index(hand)
value = values[index]
speed = speeds[index]
names.pop(index)
values.pop(index)
speeds.pop(index)
finger_names, finger_values = self._mimicHand(hand, value)
names.extend(finger_names)
values.extend(finger_values)
speeds.extend([speed]*len(finger_names))
RobotVirtual.setAngles(
self,
names,
values,
speeds)
def unsubscribeImu(self):
"""
Unsubscribe from the Inertial Unit of the robot. If the robot doesn't
have an inertial unit, the method will raise a pybullet error.
This method stops the background process collecting the Imu data, and
should be called when the Imu data are not useful anymore
"""
try:
assert self.imu is not None
self.imu.unsubscribe()
except AssertionError:
raise pybullet.error("No IMU could be found for the virtual robot")
def getCameraFrame(self):
"""
Returns a camera frame. Be advised that the subscribeCamera method
needs to be called beforehand, otherwise a pybullet error will be
raised.
Returns:
frame - The current camera frame as a formatted numpy array,
directly exploitable from OpenCV
"""
try:
assert self.active_camera is not None
return self.active_camera.getFrame()
except AssertionError:
raise pybullet.error("No active camera, cannot retrieve any frame")
def getCameraLink(self):
"""
Returns the link of the active camera. Be advised that the
subscribeCamera method needs to be called beforehand, otherwise a
pybullet error will be raised.
Returns:
resolution - a Link object describing the link to which the active
camera is attached
"""
try:
assert self.active_camera is not None
return self.active_camera.getCameraLink()
except AssertionError:
raise pybullet.error("No active camera, cannot retrieve any link")
def getImuAccelerometerValues(self):
"""
Returns the linear acceleration of the IMU in m/s^2 in the world frame.
If the robot doesn't have an inertial unit, the method will raise a
pybullet error.
One should subscribe to the IMU before calling this method (otherwise,
the acceleration will constantly be [0, 0, 0])
Returns:
linear_acceleration - The linear acceleration in m/s^2
"""
if self.imu is not None:
return self.imu.getAccelerometerValues()
else:
raise pybullet.error("No IMU could be found for the virtual robot")
def getImuGyroscopeValues(self):
"""
Returns the angular velocity of the IMU in rad/s in the world frame. If
the robot doesn't have an inertial unit, the method will raise a
pybullet error.
One should subscribe to the IMU before calling this method (otherwise,
the speed will constantly be [0, 0, 0])
Returns:
angular_velocity - The angular velocity in rad/s
"""
if self.imu is not None:
return self.imu.getGyroscopeValues()
else:
raise pybullet.error("No IMU could be found for the virtual robot")
def getCameraResolution(self):
"""
Returns the resolution of the active camera. Be advised that the
subscribeCamera method needs to be called beforehand, otherwise a
pybullet error will be raised.
Returns:
resolution - a CameraResolution object describing the resolution of
the active camera
"""
try:
assert self.active_camera is not None
return self.active_camera.getResolution()
except AssertionError:
raise pybullet.error("No active camera, resolution unavailable")
def getCameraFps(self, handle):
"""
Returns the framerate of the camera associated to the specified handle.
Be advised that the subscribeCamera method needs to be called
beforehand, otherwise a pybullet error will be raised.
Parameters:
handle - the handle retrieved when subscribing to the camera
Returns:
fps - The framerate of the camera (frequency of the camera in Hz)
"""
try:
return Camera._getCameraFromHandle(handle).getFps()
except KeyError:
raise pybullet.error("Invalid handle, framerate unavailable")
def setLightPosition(self, physics_client, light_position):
"""
Set the position of the GUI's light (does not work in DIRECT mode)
Parameters:
light_position - List containing the 3D positions [x, y, z] along
the X, Y, and Z axis in the world frame, in meters
"""
try:
assert isinstance(light_position, list)
assert len(light_position) == 3
pybullet.configureDebugVisualizer(lightPosition=light_position,
physicsClientId=physics_client)
except AssertionError:
raise pybullet.error("Incorrect light position format")
def getImuValues(self):
"""
Returns the values of the gyroscope and the accelerometer of the IMU
(angular_velocity, linear_acceleration) in the world frame. If the
robot doesn't have an inertial unit, the method will raise a pybullet
error.
One should subscribe to the IMU before calling this method (otherwise,
the acceleration and speed will constantly be [0, 0, 0], [0, 0, 0])
Returns:
angular_velocity - The angular velocity values in rad/s
linear_acceleration - The linear acceleration values in m/s^2
"""
if self.imu is not None:
return self.imu.getValues()
else:
raise pybullet.error("No IMU could be found for the virtual robot")
def getCamera(self, handle):
"""
Returns the robot camera associated to the specidied handle. Be advised
that the subscribeCamera method needs to be called beforehand,
otherwise a pybullet error will be raised.
Parameters:
handle - The handle retrieved when subscribing to the camera
Returns:
camera - Camera (CameraRgb or CameraDepth) object, the
robot camera associated to the handle.
"""
try:
return Camera._getCameraFromHandle(handle)
except KeyError:
raise pybullet.error("Invalid handle, no associated camera")
def getCameraLink(self, handle):
"""
Returns the link of the camera associated to the specified handle. Be
advised that the subscribeCamera method needs to be called beforehand,
otherwise a pybullet error will be raised.
Parameters:
handle - The handle retrieved when subscribing to the camera
Returns:
link - a Link object describing the link to which the robot camera
associated to the handle is attached
"""
try:
return Camera._getCameraFromHandle(handle).getCameraLink()
except KeyError:
raise pybullet.error("Invalid handle, cannot retrieve any link")
def getCameraResolution(self, handle):
"""
Returns the resolution of the camera associated to the specified
handle. Be advised that the subscribeCamera method needs to be called
beforehand, otherwise a pybullet error will be raised.
Parameters:
handle - The handle retrieved when subscribing to the camera
Returns:
resolution - a CameraResolution object describing the resolution of
the robot camera associated to the handle
"""
try:
return Camera._getCameraFromHandle(handle).getResolution()
except KeyError:
raise pybullet.error("Invalid handle, resolution unavailable")
def getCameraFrame(self, handle):
"""
Returns a frame form the camera associated to the specified handle
object. Be advised that the subscribeCamera method needs to be called
beforehand, otherwise a pybullet error will be raised.
Parameters:
handle - The handle retrieved when subscribing to the camera
Returns:
frame - The current camera frame as a formatted numpy array,
directly exploitable from OpenCV
"""
try:
return Camera._getCameraFromHandle(handle).getFrame()
except KeyError:
raise pybullet.error("Invalid handle, cannot retrieve any frame")
def getTotalFsrValues(self, fsr_names):
"""
Returns the total weight value (the sum of all FSRs corresponding to
the names passed to the method). If no names are specified, the method
will return 0.0. If one of the required FSR does not exist, or if
no FSR handler has been defined for the robot, the method will raise a
pybullet error
Parameters:
fsr_names - List containing the FSR names (for instance
NaoFsr.LFOOT)
Returns:
total_weight - The sum of all values for the corresponding FSRs
"""
if self.fsr_handler is not None:
return self.fsr_handler.getTotalValue(fsr_names)
else:
raise pybullet.error("No FSR handler could be found for the robot")
def isSelfColliding(self, link_names):
"""
Specifies if a link is colliding with the rest of the virtual robot.
Parameters:
link_names - String or list of string containing the names of the
links to be checked for self collision. WARNING: only the links
with corresponding meshes should be used, otherwise the link cannot
self collide
Returns:
self_colliding - Boolean, if True at least one of the links is self
colliding
"""
try:
if type(link_names) is str:
assert link_names in self.link_dict.keys()
names = [link_names]
else:
assert set(link_names).issubset(self.link_dict.keys())
names = list(link_names)
for name in names:
contact_tuple = pybullet.getContactPoints(
bodyA=self.robot_model,
bodyB=self.robot_model,
linkIndexA=self.link_dict[name].getIndex(),
physicsClientId=self.physics_client)
contact_tuple += pybullet.getContactPoints(
bodyA=self.robot_model,
bodyB=self.robot_model,
linkIndexB=self.link_dict[name].getIndex(),
physicsClientId=self.physics_client)
if len(contact_tuple) != 0:
return True
return False
except AssertionError:
raise pybullet.error(
"Unauthorized link checking for self collisions")
def setAngles(self, joint_names, joint_values, percentage_speeds):
"""
Set angles on the robot's joints. Tests have to be performed by the
child class to guarantee the validity of the input parameters.
Parameters:
joint_names - List of string containing the name of the joints
to be controlled
joint_values - List of values corresponding to the angles in
radians to be applied
percentage_speeds - Percentages of the max speed to be used for
each joint, has to be strictly superior to 0 and inferior or equal
to 1
"""
try:
assert len(joint_names) ==\
len(joint_values) ==\
len(percentage_speeds)
assert all(
speed >= 0.0 and speed <= 1.0 for speed in percentage_speeds)
except AssertionError:
raise pybullet.error("Error in the setAngles parameters")
for joint_name, joint_value, percentage_speed in zip(
joint_names,
joint_values,
percentage_speeds):
joint_speed =\
self.joint_dict[joint_name].getMaxVelocity() *\
percentage_speed
pybullet.setJointMotorControl2(
self.robot_model,
self.joint_dict[joint_name].getIndex(),
pybullet.POSITION_CONTROL,
targetPosition=joint_value,
maxVelocity=joint_speed,
force=self.joint_dict[joint_name].getMaxEffort(),
physicsClientId=self.physics_client)
def getFsrValues(self, fsr_names):
"""
Returns all of the FSR weight values for the FSRs corresponding to the
passed names. If the list of passed names is empty, the method will
return an empty list. If one of the required FSR does not exist, or if
no FSR handler has been defined for the robot, the method will raise a
pybullet error
Parameters:
fsr_names - List containing the FSR names (for instance
NaoFsr.LFOOT)
Returns:
fsr_values - The measured values for the corresponding FSRs, as a
List
"""
if self.fsr_handler is not None:
return self.fsr_handler.getValues(fsr_names)
else:
raise pybullet.error("No FSR handler could be found for the robot")
def setFrequency(self, frequency):
"""
Sets the frequency of the sensor. The expected value should be strictly
positive int or float, otherwise the method will raise a pybullet error
Parameters:
frequency - The frequency of the sensor in Hz
"""
if isinstance(frequency, int):
frequency = float(frequency)
try:
assert isinstance(frequency, float)
assert frequency > 0.0
self.frequency = frequency
except AssertionError:
raise pybullet.error(
"Incorrect frequency value, strictly positive int or float " +
"expected")
def _setResolution(self, resolution):
"""
INTERNAL METHOD, sets the resolution for the camera. The fov of the
camera has to be setted beforehand
Parameters:
resolution - The resolution of the camera (CameraResolution type)
"""
try:
with self.resolution_lock:
assert isinstance(resolution, CameraResolution)
assert self.hfov is not None and self.vfov is not None
self.resolution = resolution
self.projection_matrix = pybullet.computeProjectionMatrix(
left=-math.tan(math.pi * self.hfov / 360.0) *
self.near_plane,
right=math.tan(math.pi * self.hfov / 360.0) *
self.near_plane,
bottom=-math.tan(math.pi * self.vfov / 360.0) *
self.near_plane,
top=math.tan(math.pi * self.vfov / 360.0) *
self.near_plane,
nearVal=self.near_plane,
farVal=self.far_plane,
physicsClientId=self.physics_client)
self.intrinsic_matrix = [
self.projection_matrix[0] * self.resolution.width / 2.0,
0.0, (1 - self.projection_matrix[8]) *
self.resolution.width / 2.0, 0.0,
self.projection_matrix[5] * self.resolution.height / 2.0,
(1 - self.projection_matrix[9]) * self.resolution.height /
2.0, 0.0, 0.0, 1.0
]
except AssertionError as e:
raise pybullet.error("Cannot set camera resolution: " + str(e))
def getFsrValue(self, fsr_name):
"""
Returns the weight detected on the Z axis of the specified FSR. The
return value is given in kg (computed from the measured force on the Z
axis and the gravity of the simulation). If the required fsr does not
exist, or if no FSR handler has been defined for the robot, the method
will raise a pybullet error
WARNING: The returned value is an approximation. Good practice: instead
of the value itself, take into account the variation of the value, in
order to detect any change at foot contact level.
Parameters:
fsr_name - The name of the FSR, as a string (for instance
NaoFsr.LFOOT_FL or "LFsrFL_frame")
Returns:
fsr_value - The measured value
"""
if self.fsr_handler is not None:
return self.fsr_handler.getValue(fsr_name)
else:
raise pybullet.error("No FSR handler could be found for the robot")
def getValue(self, fsr_name):
"""
Returns the weight detected on the Z axis of the specified FSR. The
return value is given in kg (computed from the measured force on the Z
axis and the gravity of the simulation). If the required FSR does not
exist, the method will raise a pybullet error
WARNING: The returned value is an approximation. Good practice: instead
of the value itself, take into account the variation of the value, in
order to detect any change at foot contact level.
Parameters:
fsr_name - The name of the FSR, as a string (for instance
NaoFsr.LFOOT_FL or "LFsrFL_frame")
Returns:
fsr_value - The measured value
"""
try:
return self.fsr_dict[fsr_name].getValue()
except KeyError:
raise pybullet.error("The required Fsr does not exist")
