I just checked the code and it seems that the notation we are using to define the robot orientation is misleading.
def _load_environment(self, world_file, robot_file_abs):
Loads the environment and robot in Gazebo
:param world_file Backwards compatibility for world file specified through webpage
# load the world file if provided first
self._notify("Loading experiment environment")
w_models, w_lights = self._gazebo_helper.parse_gazebo_world_file(world_file)
# Create interfaces to Gazebo
robot_initial_pose = self.exc.environmentModel.robotPose
if robot_initial_pose is not None:
rpose = Pose()
rpose.position.x = robot_initial_pose.x
rpose.position.y = robot_initial_pose.y
rpose.position.z = robot_initial_pose.z
rpose.orientation.x = robot_initial_pose.ux
rpose.orientation.y = robot_initial_pose.uy
rpose.orientation.z = robot_initial_pose.uz
rpose.orientation.w = robot_initial_pose.theta
rpose = None
def load_sdf_entity(self, model_name, model_sdf, initial_pose=None):
Load a gazebo model (sdf) into the ROS connected running gazebo instance.
:param model_name: Name of the model (can be anything).
:param model_sdf: The SDF xml code describing the model.
:param initial_pose: Initial pose of the model. Uses the Gazebo \
# We are checking here that light_sdf is indeed an XML string, fromstring() raises
# exception if the parameter is not a valid XML.
# set initial pose
if initial_pose is None:
initial_pose = Pose()
initial_pose.position = Point(0, 0, 0)
initial_pose.orientation = Quaternion(0, 0, 0, 1)
# spawn model
self.spawn_entity_proxy(model_name, model_sdf, "", initial_pose, "world")
All this means that rpose.orientation = initial_pose.orientation is a quaternion
q = x*i + y*j + z*k + w = ux*i + uy*j + uz*k + theta.
This would explain why you don't get the expected result when changing uz: the rotation axis is always the same, namely the line passing through zero and directed along (0,0,1), but the rotation angle should vary according to uz.
As a reminder (see e.g., https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation), we have:
q = x*i + y*j + z*k + w = sin(theta/2) * (ux*i + uy*j + uz*k) + cos(theta)
where theta is the rotation angle and (ux, uy, uz) is the unit vector defining the rotation axis.
The problem is that the user thinks she/he is giving (ux, uy,uz, theta) as an input while she/he is actually specifying (x, y, z, w)!
In invite you to report the issue here
so that your problem is taken care of rapidly.