Merge branch 'production' of github.com:rdiankov/openrave into suppor…

…t_robotControllerAxisManufacturerCode
rdiankov · Mar 18, 2024 · 727c88d · 727c88d
2 parents 5e7ac62 + bc16ced
commit 727c88d
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 7 deletions.
diff --git a/plugins/grasper/graspermodule.cpp b/plugins/grasper/graspermodule.cpp
@@ -1016,7 +1016,7 @@ class GrasperModule : public ModuleBase
                         RAVELOG_VERBOSE(str(boost::format("contact %s\n")%report->__str__()));
                         for(int icollision = 0; icollision < report->nNumValidCollisions; ++icollision) {
                             const CollisionPairInfo& cpinfo = report->vCollisionInfos[icollision];
-                            bool bFirstMatchesRobot = cpinfo.CompareFirstBodyName(probot->GetName());
+                            bool bFirstMatchesRobot = cpinfo.CompareFirstBodyName(probot->GetName()) == 0;
                             for(const CONTACT& c : cpinfo.contacts) {
                                 if( bFirstMatchesRobot ) {
                                     grasp_params->contacts.emplace_back(c, (*itlink)->GetIndex());

diff --git a/plugins/ikfastsolvers/ikfastsolver.cpp b/plugins/ikfastsolvers/ikfastsolver.cpp
@@ -570,7 +570,7 @@ for numBacktraceLinksForSelfCollisionWithNonMoving numBacktraceLinksForSelfColli
             if( !bIndependentLink2 && !bChildLink2 && !cpinfo.bodyLinkGeom2Name.empty() ) {
                 FOREACH(it,_listGrabbedSavedStates) {
                     const KinBodyPtr& pbody = it->GetBody();
-                    if( !!pbody && cpinfo.CompareSecondBodyName(pbody->GetName()) ) {
+                    if( !!pbody && cpinfo.CompareSecondBodyName(pbody->GetName()) == 0 /*equal*/ ) {
                         if( !_bCheckEndEffectorEnvCollision ) {
                             // if bodyLinkGeom1Name is not part of the robot, then ignore.
                             if( cpinfo.bodyLinkGeom1Name.empty() || cpinfo.CompareFirstBodyName(_probot->GetName()) != 0 ) {
@@ -587,7 +587,7 @@ for numBacktraceLinksForSelfCollisionWithNonMoving numBacktraceLinksForSelfColli
             if( !bIndependentLink1 && !bChildLink1 && !cpinfo.bodyLinkGeom1Name.empty() ) {
                 FOREACH(it,_listGrabbedSavedStates) {
                     const KinBodyPtr& pbody = it->GetBody();
-                    if( !!pbody && cpinfo.CompareFirstBodyName(pbody->GetName()) ) {
+                    if( !!pbody && cpinfo.CompareFirstBodyName(pbody->GetName()) == 0 /*equal*/ ) {
                         if( !_bCheckEndEffectorEnvCollision ) {
                             // if plink2 is not part of the robot, then ignore. otherwise it needs to be counted as self-collision
                             if( cpinfo.bodyLinkGeom2Name.empty() || cpinfo.CompareSecondBodyName(_probot->GetName()) != 0 ) {

diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp
@@ -2193,21 +2193,21 @@ void KinBody::SetDOFValues(const dReal* pJointValues, int dof, uint32_t checklim
                         if( p[i] < lowerlimit ) {
                             if( p[i] < lowerlimit-g_fEpsilonEvalJointLimit ) {
                                 if( checklimits == CLA_CheckLimits ) {
-                                    RAVELOG_WARN(str(boost::format("env=%d, dof %d value %e is smaller than the lower limit %e")%GetEnv()->GetId()%(joint.GetDOFIndex()+i)%p[i]%lowerlimit));
+                                    RAVELOG_WARN(str(boost::format("env=%s, joint '%s' dofindex %d value %.16e is smaller than the lower limit %.16e")%GetEnv()->GetNameId()%joint.GetName()%(joint.GetDOFIndex()+i)%p[i]%lowerlimit));
                                 }
                                 else if( checklimits == CLA_CheckLimitsThrow ) {
-                                    throw OPENRAVE_EXCEPTION_FORMAT(_("env=%d, dof %d value %e is smaller than the lower limit %e"), GetEnv()->GetId()%(joint.GetDOFIndex()+i)%p[i]%lowerlimit, ORE_InvalidArguments);
+                                    throw OPENRAVE_EXCEPTION_FORMAT(_("env=%s, joint '%s' dofindex %d value %.16e is smaller than the lower limit %.16e"), GetEnv()->GetNameId()%joint.GetName()%(joint.GetDOFIndex()+i)%p[i]%lowerlimit, ORE_InvalidArguments);
                                 }
                             }
                             *ptempjoints++ = lowerlimit;
                         }
                         else if( p[i] > upperlimit ) {
                             if( p[i] > upperlimit+g_fEpsilonEvalJointLimit ) {
                                 if( checklimits == CLA_CheckLimits ) {
-                                    RAVELOG_WARN_FORMAT("env=%d, dof %d value %.16e is greater than the upper limit %.16e", GetEnv()->GetId()%(joint.GetDOFIndex()+i)%p[i]%upperlimit);
+                                    RAVELOG_WARN_FORMAT("env=%s, joint '%s' dofindex %d value %.16e is greater than the upper limit %.16e", GetEnv()->GetNameId()%joint.GetName()%(joint.GetDOFIndex()+i)%p[i]%upperlimit);
                                 }
                                 else if( checklimits == CLA_CheckLimitsThrow ) {
-                                    throw OPENRAVE_EXCEPTION_FORMAT(_("env=%d, dof %d value %.16e is greater than the upper limit %.16e"), GetEnv()->GetId()%(joint.GetDOFIndex()+i)%p[i]%upperlimit, ORE_InvalidArguments);
+                                    throw OPENRAVE_EXCEPTION_FORMAT(_("env=%s, joint '%s' dofindex %d value %.16e is greater than the upper limit %.16e"), GetEnv()->GetNameId()%joint.GetName()%(joint.GetDOFIndex()+i)%p[i]%upperlimit, ORE_InvalidArguments);
                                 }
                             }
                             *ptempjoints++ = upperlimit;