[Personalrobots-commit] SF.net SVN: personalrobots:[10194] pkg/trunk

[<is...@us...>]

Revision: 10194
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10194&view=rev
Author:   isucan
Date:     2009-01-26 08:23:29 +0000 (Mon, 26 Jan 2009)

Log Message:
-----------
fixed bug in state retrieval

Modified Paths:
--------------
    pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
    pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
    pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp

Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h	2009-01-26 07:54:56 UTC (rev 10193)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h	2009-01-26 08:23:29 UTC (rev 10194)
@@ -156,7 +156,8 @@
 	    m_kmodel->reduceToRobotFrame();
 	    
 	    m_robotState = m_kmodel->newStateParams();
-	    
+	    m_robotState->setInRobotFrame();
+
 	    m_haveMechanismState = false;
 	    m_haveBasePos = false;
 	}
@@ -251,9 +252,22 @@
 		unsigned int n = m_mechanismState.get_joint_states_size();
 		for (unsigned int i = 0 ; i < n ; ++i)
 		{
-		    double pos = m_mechanismState.joint_states[i].position;
-		    change = change || m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+		    planning_models::KinematicModel::Joint* joint = m_kmodel->getJoint(m_mechanismState.joint_states[i].name);
+		    if (joint)
+		    {
+			if (joint->usedParams == 1)
+			{
+			    double pos = m_mechanismState.joint_states[i].position;
+			    bool this_changed = m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+			    change = change || this_changed;
+			}
+			else
+			    ROS_WARN("Incorrect number of parameters: %s (expected %d, had 1)", m_mechanismState.joint_states[i].name.c_str(), joint->usedParams);
+		    }
+		    else
+			ROS_ERROR("Unknown joint: %s", m_mechanismState.joint_states[i].name.c_str());
 		}
+		
 		if (!m_haveMechanismState)
 		    m_haveMechanismState = m_robotState->seenAll();
 	    }

Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h	2009-01-26 07:54:56 UTC (rev 10193)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h	2009-01-26 08:23:29 UTC (rev 10194)
@@ -470,6 +470,9 @@
 	    /** Set all the parameters to a given value */
 	    void setAll(const double value);
 
+	    /** Set all planar & floating joints to 0, so that the robot is in its own frame */
+	    void setInRobotFrame(void);
+	    
 	    /** Set the parameters for a given group. Return true if
 		any change was observed in either of the set
 		values. */

Modified: pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp	2009-01-26 07:54:56 UTC (rev 10193)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp	2009-01-26 08:23:29 UTC (rev 10194)
@@ -669,6 +669,21 @@
     }    
 }
 
+void planning_models::KinematicModel::StateParams::setInRobotFrame(void)
+{
+    for (unsigned int j = 0 ; j < m_owner->floatingJoints.size() ; ++j)
+    {
+	double vals[7] = {0, 0, 0, 0, 0, 0, 1};
+	setParams(vals, m_name[m_owner->floatingJoints[j]]);
+    }
+    
+    for (unsigned int j = 0 ; j < m_owner->planarJoints.size() ; ++j)
+    {
+	double vals[3] = {0, 0, 0};
+	setParams(vals, m_name[m_owner->planarJoints[j]]);
+    }
+}
+
 const double* planning_models::KinematicModel::StateParams::getParams(void) const
 {
     return m_params;


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.