common::URI example request
Hi All,
I am quite new to Gazebo and C++. I want to get the full name of a joint, can I use the URI for that like:
common::URI uri = joint->URI();
ROS_INFO("jointname = %s", uri->Str());
I don't know how to use common::URI
Point is that I want to access a joint in another function like:
std::string jointName = this->modelPtr->GetName() + "leftjoint";
physics::JointPtr jointPtr = this->modelPtr->GetJoint(jointName);
Thanks, Peter
Asked by PeterHer on 2019-10-11 05:49:00 UTC
Answers
The string you mentioned in your comment is obtained by
std::string jointName = this->modelPtr->GetName() + "::leftjoint";
You can either pass the joint name to the plugin in SDF tags of the plugin or search the model for all the joints. Depending on your application.
Asked by kumpakri on 2019-10-11 06:47:12 UTC
Comments
Thanks for your reply.
Problem is that I create a joint in a Sensor Plugin by:
physics::PhysicsEnginePtr engine = myWorldPtr->Physics();
physics::JointPtr joint = engine->CreateJoint("fixed", myRobotModelPtr);
joint->SetName("leftjoint");
joint->Load(linkBallPtr, linkPalmPtr, ignition::math::Pose3d());
joint->Attach(linkBallPtr, linkPalmPtr);
joint->SetAxis(0, ignition::math::Vector3d(0,0,0));
joint->SetUpperLimit(0, 0);
joint->SetParam("cfm", 0, 0);
joint->SetParam("erp", 0, 0.2);
In a World Plugin I do:
std::vector<physics::JointPtr> jointVec = myRobot1ModelPtr->GetJoints();
for(std::vector<physics::JointPtr>::size_type i = 0; i != jointVec.size(); i++)
{
ROS_INFO("RifSimulatorPlugin jointVec name = %s", jointVec[i]->GetName().c_str());
}
Newly created joint is not displayed.
The full code is of the TEST Sensor Plugin OnUpdate :
// Get all the contacts.
msgs::Contacts contacts;
contacts = this->contactSensor->Contacts();
for (unsigned int i = 0; i < contacts.contact_size(); ++i)
{
uint32_t parentId = this->mySensorPtr->ParentId();
ROS_INFO("ContactPlugin::OnUpdate ParentId = %d", parentId);
std::string parentName = this->mySensorPtr->ParentName();
ROS_INFO("ContactPlugin::OnUpdate ParentName = %s", this->mySensorPtr->ParentName().c_str());
// Pointer to the world
std::string worldName = this->mySensorPtr->WorldName();
ROS_INFO("ContactPlugin::OnUpdate WorldName = %s", worldName.c_str());
physics::WorldPtr myWorldPtr = physics::get_world(worldName);
if (myWorldPtr != NULL)
{
ROS_INFO("ContactPlugin::OnUpdate myWorldPtr found");
physics::LinkPtr linkBallPtr;
physics::LinkPtr linkPalmPtr;
physics::ModelPtr myBallModelPtr = myWorldPtr->ModelByName("ball");
if (myBallModelPtr != NULL)
{
ROS_INFO("ContactPlugin::OnUpdate myBallModelPtr found");
linkBallPtr = myBallModelPtr->GetLink("ball::RIF_RoboCup_MSL_Version_20.0_Ball::ball");
if (linkBallPtr != NULL)
{
ROS_INFO("ContactPlugin::OnUpdate linkBallPtr found");
}
}
physics::ModelPtr myRobotModelPtr = myWorldPtr->ModelByName("rifrobot1") ;
if (myRobotModelPtr != NULL)
{
ROS_INFO("ContactPlugin::OnUpdate myRobotModelPtr found");
linkPalmPtr = myRobotModelPtr->GetLink(parentName);
if (linkPalmPtr != NULL)
{
ROS_INFO("ContactPlugin::OnUpdate linkPalmPtr found");
}
}
if (linkBallPtr != NULL && linkPalmPtr != NULL)
{
ROS_INFO("ContactPlugin::OnUpdate both links found");
// create prismatic joint with the world as a parent
physics::PhysicsEnginePtr engine = myWorldPtr->Physics();
physics::JointPtr joint = engine->CreateJoint("fixed", myRobotModelPtr);
joint->SetName("leftjoint");
joint->Load(linkBallPtr, linkPalmPtr, ignition::math::Pose3d());
joint->Attach(linkBallPtr, linkPalmPtr);
joint->SetAxis(0, ignition::math::Vector3d(0,0,0));
joint->SetUpperLimit(0, 0);
joint->SetParam("cfm", 0, 0);
joint->SetParam("erp", 0, 0.2);
common::URI uri = joint->URI();
ROS_INFO("jointname URI = %s", uri.Str().c_str());
sdf::ElementPtr elementPtr = joint->GetSDF();
ROS_INFO("jointname Element = %s", elementPtr->GetName().c_str());
physics::LinkPtr linkPtr = joint->GetJointLink(0);
ROS_INFO("jointname JointLink = %s", linkPtr->GetName().c_str());
std::vector<physics::JointPtr> jointVec = myRobotModelPtr->GetJoints();
for(std::vector<physics::JointPtr>::size_type i = 0; i != jointVec.size(); i++)
{
ROS_INFO("jointVec name = %s", jointVec[i]->GetName().c_str());
}
}
}
std::cout << "Collision between[" << contacts.contact(i).collision1()
<< "] and [" << contacts.contact(i).collision2() << "]\n";
for (unsigned int j = 0; j < contacts.contact(i).position_size(); ++j)
{
std::cout << j << " Position:"
<< contacts.contact(i).position(j).x() << " "
<< contacts.contact(i).position(j).y() << " "
<< contacts.contact(i).position(j).z() << "\n";
std::cout << " Normal:"
<< contacts.contact(i).normal(j).x() << " "
<< contacts.contact(i).normal(j).y() << " "
<< contacts.contact(i).normal(j).z() << "\n";
std::cout << " Depth:" << contacts.contact(i).depth(j) << "\n";
}
Output is: [ INFO] [1570798160.005631892, 2.008000000]: ContactPlugin::OnUpdate ParentId = 243 [ INFO] [1570798160.005648202, 2.008000000]: ContactPlugin::OnUpdate ParentName = rifrobot1::mobile_base::simple_gripper::palm [ INFO] [1570798160.005666528, 2.008000000]: ContactPlugin::OnUpdate WorldName = RIF_RoboCup_Simulator [ INFO] [1570798160.005678202, 2.008000000]: ContactPlugin::OnUpdate myWorldPtr found [ INFO] [1570798160.005728852, 2.008000000]: ContactPlugin::OnUpdate myBallModelPtr found [ INFO] [1570798160.005743042, 2.008000000]: ContactPlugin::OnUpdate linkBallPtr found [ INFO] [1570798160.005777352, 2.008000000]: ContactPlugin::OnUpdate myRobotModelPtr found [ INFO] [1570798160.005797026, 2.008000000]: ContactPlugin::OnUpdate linkPalmPtr found [ INFO] [1570798160.005811033, 2.008000000]: ContactPlugin::OnUpdate both links found [ INFO] [1570798160.006590100, 2.009000000]: jointname URI = data://world/RIF_RoboCup_Simulator/model/rifrobot1/joint/leftjoint [ INFO] [1570798160.006617278, 2.009000000]: jointname Element = joint [ INFO] [1570798160.006635732, 2.009000000]: jointname JointLink = mobile_base::simple_gripper::palm [ INFO] [1570798160.006666443, 2.009000000]: jointVec name = mobile_base::left_wheel_hinge [ INFO] [1570798160.006683851, 2.009000000]: jointVec name = mobile_base::right_wheel_hinge [ INFO] [1570798160.006701089, 2.009000000]: jointVec name = mobile_base::simple_gripper::palm_left_finger [ INFO] [1570798160.006717300, 2.009000000]: jointVec name = mobile_base::simple_gripper::left_finger_tip [ INFO] [1570798160.006731115, 2.009000000]: jointVec name = mobile_base::simple_gripper::palm_right_finger [ INFO] [1570798160.006744060, 2.009000000]: jointVec name = mobile_base::simple_gripper::right_finger_tip [ INFO] [1570798160.006756832, 2.009000000]: jointVec name = mobile_base::simple_gripper::palm_riser [ INFO] [1570798160.006770153, 2.009000000]: jointVec name = mobile_base::gripper_joint [ INFO] [1570798160.006786146, 2.009000000]: jointVec name = mobile_base::camera_joint Collision between[ball::RIF_RoboCup_MSL_Version_20.0_Ball::ball::collision] and [rifrobot1::mobile_base::simple_gripper::palm::palm_collision] 0 Position:-0.107297 2.65855e-05 0.118285 Normal:1 -0.000254764 -4.7322e-05 Depth:1.95513e-06
As you can see, no leftjoint.
Asked by PeterHer on 2019-10-11 07:44:46 UTC
Comments
I think I have to use myRobotModelPtr->CreateJoint() ??
Asked by PeterHer on 2019-10-11 07:48:40 UTC
Sorry for my ignorance;
Solution is: myRobotModelPtr->CreateJoint("leftjoint", "fixed", linkPalmPtr, linkBallPtr);
Output: [ INFO] [1570798602.675884747, 1.653000000]: jointVec name = leftjoint
Asked by PeterHer on 2019-10-11 07:52:54 UTC
Comments
Solution:
But unfortunately this gives me something other than:
robot::base::leftjointI need something like:robot::base::leftjointWhich method to use?
Asked by PeterHer on 2019-10-11 06:09:07 UTC