Revision history [back]

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: 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.