Robot Pose Rotation useless?


#1

Hey Guys,

i recently tried to rotate the robot through the exc file:

robotPose x=“0.0” y=“0.0” z=“0.35” ux=“0.0” uy=“0.0” uz=“90” theta=“0.70710689672598181”/>

(btw, when posting code here trying to use the < character after the > for code indentation ignores all the code after it, hence the missing < before robotPose)

but whatever i put in the uz="" part, it is completely ignored.
The same is true for rotations defined in the environment sdf files. Just create a model (box or whatever) and try to set the rotation of the pose, they get ignored as well.

pose frame=’’>0 2.3 1.4 0 0 90

Regards,
Ben


#2

Dear Ben,

I just checked the code and it seems that the notation we are using to define the robot orientation is misleading.

From ExDBackend/hbp_nrp_cleserver/hbp_nrp_cleserver/CLEGazeboSimulationAssembly.py:

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
    self._notify("Loading robot")

    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
    else:
        rpose = None

From CLE/hbp_nrp_cle/robotsim/GazeboHelper.py:

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 \
        "Pose" type.
    """
    # We are checking here that light_sdf is indeed an XML string, fromstring() raises
    # exception if the parameter is not a valid XML.
    etree.fromstring(model_sdf)
    # 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

https://bitbucket.org/hbpneurorobotics/neurorobotics-platform/issues/new

so that your problem is taken care of rapidly.

Best regards,
Luc


#3

Hey Luc,
thanks for the insight, i created an issue, for future reference see here:

https://bitbucket.org/hbpneurorobotics/neurorobotics-platform/issues/55/robot-pose-rotation-not-working-as

Regards,
Ben