[ playerstage-Bugs-3046548 ] Gazebo model's position update

Bugs item #3046548, was opened at 2010-08-16 15:01
Message generated for change (Tracker Item Submitted) made by fmder
You can respond by visiting: 
https://sourceforge.net/tracker/?func=detail&atid=433164&aid=3046548&group_id=42445

Please note that this message will contain a full copy of the comment thread,
including the initial issue submission, for this request,
not just the latest update.
Category: gazebo
Group: None
Status: Open
Resolution: None
Priority: 5
Private: No
Submitted By: Francois-Michel De Rainville (fmder)
Assigned to: Nathan Koenig (natepak)
Summary: Gazebo model's position update

Initial Comment:
In Gazebo 0.10.0, the position of all models doesn't get updated (and cannot be retreived from SimulationProxy). This is due to a part of the Update() function that is commented. When uncommenting the function, compiler complains about the fact that this->pose doesn't existe.

The whole if statement following the comment // BULLET: (line 469 of the Model.cc file) shall be replaced by :

  if (!this->canonicalBodyNameP->GetValue().empty())
  {
    /// model pose is the canonical body pose of the body + a transform from body frame to model frame
    /// the tranform is defined by initModelOffset in body frame,

    /// recover the transform in inertial frame based on body pose
    Pose3d pose = this->bodies[**this->canonicalBodyNameP]->GetAbsPose();
    Quatern body_rot = pose.rot;
    Pose3d offset_transform = this->bodies[**this->canonicalBodyNameP]->initModelOffset;
    Vector3 xyz_offset = (offset_transform.RotatePositionAboutOrigin(body_rot.GetInverse())).pos;
    Quatern q_offset = offset_transform.rot;

    this->xyzP->SetValue(pose.pos + xyz_offset);
    this->rpyP->SetValue(pose.CoordRotationAdd(q_offset));
  }

A method GetPose() in Model.hh shall also be added like following :

public: Pose3d GetPose() const {return Pose3d(**xyzP, **rpyP);}

And in file World.cc line 1071 has to be changed for :

Pose3d pose = model->GetPose();

----------------------------------------------------------------------

You can respond by visiting: 
https://sourceforge.net/tracker/?func=detail&atid=433164&aid=3046548&group_id=42445

------------------------------------------------------------------------------
This SF.net email is sponsored by 

Make an app they can't live without
Enter the BlackBerry Developer Challenge
http://p.sf.net/sfu/RIM-dev2dev