Control of Closed-Kinematic Chain with a Plugin

edit

Ok some update, I will finally use the JointController Class. Problem is, the SetJointPosition function doesnt seem to work and i am not the only one because there is also a few threads about this already, whithout any convincing answer so far..

Even more disturbing, I found this on the doc: 'Implementation: In order to change the position of a Joint inside a Model, this call must recursively crawl through all the connected children Link's in this Model, and update each Link Pose affected by this Joint angle update. Warning: There is no constraint satisfaction being done here, traversal through the kinematic graph has unexpected behavior if you try to set the joint position of a link inside a loop structure. Warning: "

So I shoudlnt expect for the JointController to work on a closed chain. Is there any alternative??

Here is my code:

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/math/Pose.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>

namespace gazebo
{
  class ModelPush : public ModelPlugin
{

public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
  // Store the pointer to the model
  this->model = _parent;

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&ModelPush::OnUpdate, this, _1));
}

// Called by the world update start event
public: void OnUpdate(const common::UpdateInfo & /*_info*/)
{
  // Apply a small linear velocity to the model.
  //this->model->SetLinearVel(math::Vector3(.03, 0, 0));

    int i;

    physics::JointController jointController(this->model);



    //printf("test \n");
    joint = this->model->GetJoint("BaseToGround");
    joint2 = this->model->GetJoint("prismatic1");


    jointController.AddJoint(joint);
    jointController.AddJoint(joint2);
    jointController.AddJoint(this->model->GetJoint("arm2-end"));
    jointController.AddJoint(this->model->GetJoint("arm2_JOINT_0"));
    jointController.AddJoint(this->model->GetJoint("Base-verin2"));
    jointController.AddJoint(this->model->GetJoint("Base-arm1"));
    jointController.AddJoint(this->model->GetJoint("Base-verin1"));
    jointController.AddJoint(this->model->GetJoint("dummy-arm1"));
    jointController.AddJoint(this->model->GetJoint("arm1-arm2"));
    jointController.AddJoint(this->model->GetJoint("prismatic2"));      

    for(i=0; i<100; i++)
        jointController.SetJointPosition(joint2, 0.5,i);


    //for(i=0;i<100;i++)
        //joint->SetPosition(i,1);



}




private: physics::JointPtr joint;
private: physics::JointPtr joint2;
// Pointer to the model
private: physics::ModelPtr model;

// Pointer to the update event connection
private: event::ConnectionPtr updateConnection;
 };




}