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

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

Revision: 10177
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10177&view=rev
Author:   isucan
Date:     2009-01-25 20:15:59 +0000 (Sun, 25 Jan 2009)

Log Message:
-----------
option to go to robot frame

Modified Paths:
--------------
    pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
    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/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp	2009-01-25 08:11:24 UTC (rev 10176)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/displays/planning_display.cpp	2009-01-25 20:15:59 UTC (rev 10177)
@@ -165,6 +165,7 @@
   kinematic_model_->setVerbose( false );
   kinematic_model_->build( file );
   kinematic_model_->defaultState();
+  kinematic_model_->reduceToRobotFrame();
 
   robot_->update( kinematic_model_, target_frame_ );
 }

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-25 08:11:24 UTC (rev 10176)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h	2009-01-25 20:15:59 UTC (rev 10177)
@@ -548,7 +548,10 @@
 
 	void defaultState(void);
 	void computeTransforms(const double *params, int groupID = -1);
-
+	
+	/** Add thansforms to the rootTransform such that the robot is in its planar/floating link frame */
+	bool reduceToRobotFrame(void);
+	
 	void printModelInfo(std::ostream &out = std::cout);
 	void printLinkPoses(std::ostream &out = std::cout) const;
 	

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-25 08:11:24 UTC (rev 10176)
+++ pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp	2009-01-25 20:15:59 UTC (rev 10177)
@@ -322,6 +322,20 @@
 {
     return new StateParams(this);
 }
+	
+bool planning_models::KinematicModel::reduceToRobotFrame(void)
+{
+    if (m_robots.size() == 1 && m_robots[0]->floatingJoints.size() + m_robots[0]->planarJoints.size() == 1)
+    {
+	if (m_robots[0]->planarJoints.size())
+	    rootTransform *= m_jointMap[parameterValues[m_robots[0]->planarJoints[0]]]->after->constTrans.inverse();
+	else
+	    rootTransform *= m_jointMap[parameterValues[m_robots[0]->floatingJoints[0]]]->after->constTrans.inverse();
+	return true;
+    }
+    else
+	return false;
+}
 
 void planning_models::KinematicModel::build(const robot_desc::URDF &model, bool ignoreSensors)
 {


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