Home | Tutorials | Wiki | Issues
Ask Your Question
0

Giving a 180 degree rotation to a robot

asked 2013-07-22 12:21:53 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

Hi All,

I am trying to build a robot which will stop 2 units length ahead of the obstacle and will have a 180 degree (pi radian) rotation. After the rotation it will again start moving to other obstacles. For that I am writing a Gazebo Plugin with the following OnUpdate() function

public : void OnUpdate() {


double FinalVelocity = 1.0;
double minRange;
double ReducingFactor = 10.0;
double AngularPosition;
double xPositionCurr, yPositionCurr, zPositionCurr;
double xPositionFin, yPositionFin, zPositionFin;

std::vector<double> ranges;
minRange = this->laser->GetRangeMax();


this->laser->GetRanges(ranges);
for(std::vector<double>::const_iterator it = ranges.begin(); it != ranges.end(); ++it) {

  if(*it < minRange){
     minRange = *it;
  }
}
 if(minRange <= 1.0){       
   FinalVelocity = 0.0;
   this->model->SetLinearVel(math::Vector3(FinalVelocity, 0, 0));       
 }      
 else {
   double ErrorMargin  = minRange - 1.0;
   FinalVelocity = ErrorMargin / ReducingFactor;
   this->model->SetLinearVel(math::Vector3(FinalVelocity, 0, 0));
  //FinalVelocity = std::max(FinalVelocity, 0.05)
 }

//      this->model->SetLinearVel(math::Vector3(FinalVelocity, 0, 0));
if(FinalVelocity == 0.0){
  this->left_wheel_joint_->SetForce(0, 0.9);
  this->right_wheel_joint_->SetForce(0,-0.9);
}
}

The model rotates continuously. How can I ensure that after 180 degree rotation the model stops and moves forward? Thanks for your kind suggestions.

Thanks and Regards, Debjit.

edit retag flag offensive close merge delete

Comments

It looks like the minRange value is never set. Is that correct?

scpeters gravatar imagescpeters ( 2013-07-23 01:33:24 -0500 )edit

I previously posted a partial code. The complete code is here:

Debjit Pal gravatar imageDebjit Pal ( 2013-07-23 07:37:49 -0500 )edit

I previously posted a partial code. The complete code is here:

Debjit Pal gravatar imageDebjit Pal ( 2013-07-23 07:37:50 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2013-07-24 14:07:44 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

Dear my Friend, You can this work in two ways, in first way you should count number of iterations that robot needs to reaching your target degree with 0.9 speed and then define a counter in your code to checking it. it's a simple way but solve your problem. in second way you can work with z parameter in orientation of robot, and get current z of orientation and then add 180 degree to it and when your robot reaches to your target angle then send zero speed to wheels. i do this in this way, but i don't do it in a plugin. just i should say the z parameter changed between 0 to -1 when head of robot turn to right, and 0 to 1 when head of robot turned to left, 0 to 1 is equal 0 to 180. you should subscribe to "~/pose/info" topic.

subInfo  = nodeInfo->Subscribe("~/pose/info", &Manage::parseInfo, this);

void Manage::parseInfo(ConstPosesStampedPtr &_msg)
{
    if ( MGDebug )
        qDebug()<<"<<<MG>>> In ParseInfo Function.";

    QStringList listmsg;
    QString     msg(_msg->ShortDebugString().c_str()), tempMsg, rmmsg;

    // Keep A Copy of Message For Next Parse
    tempMsg = msg;

///***/// Time Extraction

    // Cut Message Before Sec
    rmmsg = "sec:";
    msg   = msg.remove(0,msg.indexOf(rmmsg));

    // Cut After nsec
    rmmsg = "nsec";
    msg   = msg.remove(msg.indexOf(rmmsg),msg.size());

    // Remove sec:
    rmmsg = "sec: ";
    msg   = msg.remove(rmmsg);

    // Convert Timefrom String to Int
    time = msg.toInt();

    if ( MGDebug )
        qDebug()<<"<<<MG>>> Parsed Time of Simulator: "<<time;

///***///

///***/// Extract Position of Robot
    msg = tempMsg;

    // Cut Message Before First Position
    rmmsg = "position";
    msg   = msg.remove(0,msg.indexOf(rmmsg));

    // Cut After First Oriention
    rmmsg = "orientation";
    msg   = msg.remove(msg.indexOf(rmmsg),msg.size());

    // Remove Other Additional Parts
    msg   = msg.remove("position { x: ");
    msg   = msg.remove("y: ");
    msg   = msg.remove("z: ");
    msg   = msg.remove(" } ");

    // Split Msg to Seperated String
    listmsg.clear();
    listmsg = msg.split(QRegExp("\\s+"));

    // Set Position to Its Varibales
    posRobot.x = QString(listmsg[0]).toDouble();
    posRobot.y = QString(listmsg[1]).toDouble();
    posRobot.z = QString(listmsg[2]).toDouble();

    if ( MGDebug )
        qDebug()<<"<<<MG>>> Parsed Position of Robot, X: "<<posRobot.x<<" , Y: "<<posRobot.y<<" , Z: "<<posRobot.z;

///***////

///***/// Extarct Orientation of Robot
    msg = tempMsg;

    // Cut Message Before First Orientation
    rmmsg = "orientation";
    msg   = msg.remove(0,msg.indexOf(rmmsg));

    // Cut After Second Pose
    rmmsg = "pose";
    msg   = msg.remove(msg.indexOf(rmmsg,2),msg.size());

    // Remove Other Additional Parts
    msg   = msg.remove("orientation { x: ");
    msg   = msg.remove("y: ");
    msg   = msg.remove("z: ");
    msg   = msg.remove("w: ");
    msg   = msg.remove(" } } ");

    // Split Msg to Seperated String
    listmsg.clear();
    listmsg = msg.split(QRegExp("\\s+"));

    // Set Position to Its Varibales
    rotRobot.x = QString(listmsg[0]).toDouble();
    rotRobot.y = QString(listmsg[1]).toDouble();
    rotRobot.z = QString(listmsg[2]).toDouble();
    rotRobot.w = QString(listmsg[3]).toDouble();

    if ( rotRobot.w < 0 )
        rotRobot.z = rotRobot.z * -1;

    if ( MGDebug )
        qDebug()<<"<<<MG>>> Parsed Rotation of Robot (Head Angle of Head), Z: "<<rotRobot.z;

///***///
}
edit flag offensive delete link more

Comments

Dear Vahid Azizi, How can I get the z parameter? Are you suggesting the z parameter from the model->GetWorldPose().pos function? If not please suggest.. Thanks for your suggestion..

Debjit Pal gravatar imageDebjit Pal ( 2013-08-03 16:52:12 -0500 )edit

Dear my friend, you should subscribe to ("/pose/info") topic and then parse message and extract the z parameter in orientation, I've added my code in main answer, in final rotRobot.z is head angle of robot. if you need more help please say.

Vahid gravatar imageVahid ( 2013-08-04 01:06:37 -0500 )edit
0

answered 2013-07-23 17:07:41 -0500

this post is marked as community wiki

This post is a wiki. Anyone with karma >75 is welcome to improve it.

One issue is that you are calling model->SetLinearVel. This works by applying an impulse to the "Canonical link" of the model that causes that link to have the desired velocity. Currently, the "Canonical link" is the first link listed in the model (see comments in Model.cc).

If your model has wheels but you set the velocity of the main robot body, the friction forces may react strongly to the impulses applied by SetLinearVell. For the example of a wheeled robot, I would recommend using SetLinearVel or SetForce on the wheels of the robot rather than the robot body.

edit flag offensive delete link more

Comments

@ scpeters, so basically you are suggesting something like this->leftwheeljoint_->SetForce(0, FinalVelocity) instead of the this->model->SelLinearVel(math::Vector3(FinalVelocity,0,0)) ? I did the following:

this->leftwheeljoint->SetForce(0, FinalVelocity); this->rightwheeljoint->SetForce(0,FinalVelocity);

But even after getting a minRange = 0, the robo dont stop.. Possibly I am doing something wrong.. Looking for your suggestions.

Debjit Pal gravatar imageDebjit Pal ( 2013-08-03 13:15:19 -0500 )edit

If you're going to do `joint-`>`SetForce()`, then you need to make the force act in opposition to the current joint velocity. You can also do `joint->SetVelocity(0, FinalVelocity)'.

scpeters gravatar imagescpeters ( 2013-08-04 12:08:01 -0500 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2013-07-22 12:21:53 -0500

Seen: 967 times

Last updated: Aug 04 '13