GetAngle don't give an exacte value
Hi every body
When I set the joint of an angle to a specific value and then I read it I don't have the same value. And each time a different value.
Example:
this->jointController->SetJointPosition(joint, 0.0);
I get the joint by:
this->joint->GetAngle(2).Radian();
and the value I get are either:
-0.002423015843935 or -0.000104254624437 or 0.000201667337713....
And if I set the angle to 0.5 I get:
0.5000000001 or 0.49999999
This really affects the results of my work. What do you think ?
thanks in advance!
EDIT
The source code
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
using namespace std;
namespace gazebo
{
class SetJoints : public ModelPlugin
{
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
this->model = _parent;
this->controller = new physics::JointController(model);
this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&SetJoints::OnUpdate, this, _1));
}
public: void OnUpdate(const common::UpdateInfo & )
{
//If I set the index it will not set the angle to specific angle
//controller->SetJointPosition(this->model->GetJoint("joint"), 2, 1);
controller->SetJointPosition(this->model->GetJoint("joint"), 1.5);
cout.precision(15);
cout << fixed << this->model->GetJoint("joint")->GetAngle(1).Radian() << endl;
}
private: physics::ModelPtr model;
private: event::ConnectionPtr updateConnection;
private: physics::JointController * controller;
};
GZ_REGISTER_MODEL_PLUGIN(SetJoints)
}
the SDF file
<?xml version='1.0'?>
<sdf version='1.4'>
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<model name="monModele">
<link name='bras1'>
<pose>0 0.4 0.025 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>.4 .4 .05</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>.4 .4 .05</size>
</box>
</geometry>
</visual>
</link>
<link name='bras2'>
<pose>0.4 0.4 0.025 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>.4 .4 .05</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>.4 .4 .05</size>
</box>
</geometry>
</visual>
</link>
<joint type="revolute" name="joint">
<pose>.2 0 0 0 0 0</pose>
<child>bras1</child>
<parent>bras2</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<joint type="revolute" name="jointGround">
<pose>0 0 -0.025 0 0 0</pose>
<parent>world</parent>
<child>bras2</child>
<axis>
<xyz>0 1 0</xyz>
<limit>
<lower>0</lower><upper>0</upper>
</limit>
</axis>
</joint>
<plugin name="set_joints" filename="build/libset_joints.so"/>
</model>
</world>
</sdf>
Is your joint interacting with physics? It might have slightly moved since you set it.
yes. I get the angle in the same update loop, so if the joint will move, it will be in next update loop isn't it ?