[Personalrobots-commit] SF.net SVN: personalrobots:[23357] pkg/trunk/motion_planning/ planning_models

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

Revision: 23357
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23357&view=rev
Author:   isucan
Date:     2009-08-30 22:31:13 +0000 (Sun, 30 Aug 2009)

Log Message:
-----------
first test that passes

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_models/CMakeLists.txt
    pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
    pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
    pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp

Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt	2009-08-30 22:14:43 UTC (rev 23356)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt	2009-08-30 22:31:13 UTC (rev 23357)
@@ -19,6 +19,6 @@
 target_link_libraries(test_kinematic planning_models)
 rospack_link_boost(test_kinematic thread)
 
-#rospack_add_gtest(test_kinematic2 test/test_kinematic2.cpp)
-#target_link_libraries(test_kinematic2 planning_models2)
-#rospack_link_boost(test_kinematic2 thread)
+rospack_add_gtest(test_kinematic2 test/test_kinematic2.cpp)
+target_link_libraries(test_kinematic2 planning_models2)
+rospack_link_boost(test_kinematic2 thread)

Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h	2009-08-30 22:14:43 UTC (rev 23356)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h	2009-08-30 22:31:13 UTC (rev 23357)
@@ -380,6 +380,12 @@
 	    max) at index positions (2*i, 2*i+1)*/
 	const std::vector<double> &getStateBounds(void) const;
 	
+	/** \brief Return a list of names of joints that are planar */
+	const std::vector<std::string> &getPlanarJoints(void) const;
+
+	/** \brief Return a list of names of joints that are floating */
+	const std::vector<std::string> &getFloatingJoints(void) const;
+	
 	/** \brief Provide interface to a lock. Use carefully! */
 	void lock(void);
 	
@@ -390,7 +396,7 @@
 	void printModelInfo(std::ostream &out = std::cout) const;
 
 	/** \brief Print the pose of every link */
-	void printLinkPoses(std::ostream &out = std::cout) const;
+	void printTransforms(std::ostream &out = std::cout) const;
 	
     private:
 	
@@ -437,6 +443,7 @@
 	Joint* constructJoint(const urdf::Joint *urdfJoint, std::vector<double> &bounds);
 	Link*  constructLink(const urdf::Link *urdfLink);
 	shapes::Shape* constructShape(const urdf::Geometry *geom);
+	void printTransform(const std::string &st, const btTransform &t, std::ostream &out = std::cout) const;
 	
     };
 

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp	2009-08-30 22:14:43 UTC (rev 23356)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp	2009-08-30 22:31:13 UTC (rev 23357)
@@ -46,16 +46,35 @@
 {    
     dimension_ = 0;
     modelName_ = model.getName();
+    rootTransform_.setIdentity();
     
     if (model.getRoot())
     {
-	root_ = buildRecursive(NULL, model.getRoot().get());
-	buildGroups(groups);
+	const urdf::Link *root = model.getRoot().get();
+	
+	if (root->name == "world")
+	{
+	    if (root->child_links.empty())
+	    {
+		root = NULL;
+		ROS_ERROR("No links connected to the world");
+	    }
+	    else
+		root = root->child_links[0].get();
+	    if (root->child_links.size() > 1)
+		ROS_WARN("More than one link connected to the world. Only considering the first one");
+	}
+	
+	if (root)
+	{
+	    root_ = buildRecursive(NULL, root);
+	    buildGroups(groups);
+	}
     }
     else
     {
 	root_ = NULL;
-	ROS_WARN("No root found");
+	ROS_WARN("No root link found");
     }
 }
 
@@ -92,6 +111,16 @@
     rootTransform_ = transform;
 }
 
+const std::vector<std::string> &planning_models::KinematicModel::getFloatingJoints(void) const
+{
+    return floatingJoints_;
+}
+
+const std::vector<std::string> &planning_models::KinematicModel::getPlanarJoints(void) const
+{
+    return planarJoints_;
+}
+
 void planning_models::KinematicModel::lock(void)
 {
     lock_.lock();
@@ -102,6 +131,22 @@
     lock_.unlock();
 }
 
+void planning_models::KinematicModel::defaultState(void)
+{
+    if (dimension_ <= 0)
+	return;
+    
+    double params[dimension_];
+    for (unsigned int i = 0 ; i < dimension_ ; ++i)
+    {
+	if (stateBounds_[2 * i] <= 0.0 && stateBounds_[2 * i + 1] >= 0.0)
+	    params[i] = 0.0;
+	else
+	    params[i] = (stateBounds_[2 * i] + stateBounds_[2 * i + 1]) / 2.0;
+    }
+    computeTransforms(params);
+}
+
 void planning_models::KinematicModel::buildGroups(const std::map< std::string, std::vector<std::string> > &groups)
 {
     for (std::map< std::string, std::vector<std::string> >::const_iterator it = groups.begin() ; it != groups.end() ; ++it)
@@ -141,6 +186,8 @@
     dimension_ += joint->usedParams;
     joint->before = parent;
     joint->after = constructLink(link);
+    if (parent == NULL)
+	joint->after->constTrans.setIdentity();
     linkMap_[joint->after->name] = joint->after;
     joint->after->before = joint;
     
@@ -265,6 +312,7 @@
 	    urdf::Vector3 dim = dynamic_cast<const urdf::Box*>(geom)->dim;
 	    result = new shapes::Box(dim.x, dim.y, dim.z);
 	}
+	break;
     case urdf::Geometry::CYLINDER:
 	result = new shapes::Cylinder(dynamic_cast<const urdf::Cylinder*>(geom)->radius,
 				      dynamic_cast<const urdf::Cylinder*>(geom)->length);
@@ -490,19 +538,32 @@
     }
 }
 
-void planning_models::KinematicModel::printLinkPoses(std::ostream &out) const
+void planning_models::KinematicModel::printTransform(const std::string &st, const btTransform &t, std::ostream &out) const
 {
+    out << st << std::endl;
+    const btVector3 &v = t.getOrigin();
+    out << "  origin: " << v.x() << ", " << v.y() << ", " << v.z() << std::endl;
+    const btQuaternion &q = t.getRotation();
+    out << "  quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl;
+}
+
+void planning_models::KinematicModel::printTransforms(std::ostream &out) const
+{
+    out << "Joint transforms:" << std::endl;
+    std::vector<const Joint*> joints;
+    getJoints(joints);
+    for (unsigned int i = 0 ; i < joints.size() ; ++i)
+    {
+	printTransform(joints[i]->name, joints[i]->varTrans);
+	out << std::endl;	
+    }
     out << "Link poses:" << std::endl;
     std::vector<const Link*> links;
     getLinks(links);
     for (unsigned int i = 0 ; i < links.size() ; ++i)
     {
-	out << links[i]->name << std::endl;
-	const btVector3 &v = links[i]->globalTrans.getOrigin();
-	out << "  origin: " << v.x() << ", " << v.y() << ", " << v.z() << std::endl;
-	const btQuaternion &q = links[i]->globalTrans.getRotation();
-	out << "  quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl;
-	out << std::endl;
+	printTransform(links[i]->name, links[i]->globalTrans);
+	out << std::endl;	
     }    
 }
 
@@ -569,11 +630,11 @@
 
 
 void planning_models::KinematicModel::Link::computeTransform(void)
-{    
+{
     globalTransFwd.mult(before->before ? before->before->globalTransFwd : owner->getRootTransform(), constTrans);
     globalTransFwd *= before->varTrans;    
     globalTrans.mult(globalTransFwd, constGeomTrans);
-        
+
     for (unsigned int i = 0 ; i < attachedBodies.size() ; ++i)
 	attachedBodies[i]->computeTransform();
 }

Modified: pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp	2009-08-30 22:14:43 UTC (rev 23356)
+++ pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp	2009-08-30 22:31:13 UTC (rev 23357)
@@ -60,17 +60,23 @@
     return i1 == s1.size() && i2 == s2.size();
 }
 
-TEST(Loading, EmptyRobot)
+TEST(Loading, SimpleRobot)
 {
     static const std::string MODEL0 = 
 	"<?xml version=\"1.0\" ?>" 
 	"<robot name=\"myrobot\">" 
 	"  <joint name=\"base_joint\" type=\"planar\">"
-	"  <origin rpy=\"0 0 0\" xyz=\"0 0 0.051\"/>"
+	"  <origin rpy=\"0 0 0\" xyz=\"0 0 0.0\"/>"
 	"  <parent link=\"world\"/>"
 	"  <child link=\"base_link\"/>"
 	"</joint>"
 	"<link name=\"base_link\">"
+	"  <collision name=\"base_collision\">"
+        "  <origin rpy=\"0 0 0\" xyz=\"0 0 0.165\"/>"
+        "  <geometry name=\"base_collision_geom\">"
+	"  <box size=\"0.65 0.65 0.23\"/>"
+        "</geometry>"
+	"</collision>"
 	"</link>"
 	"</robot>";
 
@@ -81,15 +87,15 @@
     planning_models::KinematicModel *model = new planning_models::KinematicModel(urdfModel, groups);
     
     EXPECT_EQ(std::string("myrobot"), model->getName());
-    EXPECT_EQ((unsigned int)0, model->getDimension());
+    EXPECT_EQ((unsigned int)3, model->getDimension());
     
     std::vector<const planning_models::KinematicModel::Link*> links;
     model->getLinks(links);
-    EXPECT_EQ((unsigned int)0, links.size());
+    EXPECT_EQ((unsigned int)1, links.size());
     
     std::vector<const planning_models::KinematicModel::Joint*> joints;
     model->getJoints(joints);
-    EXPECT_EQ((unsigned int)0, joints.size());
+    EXPECT_EQ((unsigned int)1, joints.size());
     
     std::vector<std::string> pgroups;
     model->getGroupNames(pgroups);    
@@ -98,7 +104,84 @@
     delete model;
 }
 
+TEST(LoadingAndFK, SimpleRobot)
+{   
+    static const std::string MODEL1 = 
+	"<?xml version=\"1.0\" ?>" 
+	"<robot name=\"one_robot\">"
+	"  <joint name=\"base_joint\" type=\"planar\">"
+	"  <parent link=\"world\"/>"
+	"  <child link=\"base_link\"/>"
+	"  <origin rpy=\"1 0 0 \" xyz=\"0 0 0 \"/>"
+	"  </joint>"
+	"<link name=\"base_link\">"
+	"  <inertial>"
+	"    <mass value=\"2.81\"/>"
+	"    <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>"
+	"    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
+	"  </inertial>"
+	"  <collision name=\"my_collision\">"
+	"    <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>"
+	"    <geometry>"
+	"      <box size=\"1 2 1\" />"
+	"    </geometry>"
+	"  </collision>"
+	"  <visual>"
+	"    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
+	"    <geometry>"
+	"      <box size=\"1 2 1\" />"
+	"    </geometry>"
+	"  </visual>"
+	"</link>"
+	"</robot>";
+    
+    static const std::string MODEL1_INFO = 
+	"Complete model state dimension = 3\n"
+	"State bounds: [0.00000, 0.00000] [0.00000, 0.00000] [-3.14159, 3.14159] \n"
+	"Floating joints : \n"
+	"Planar joints : base_joint \n"
+	"Available groups: base \n"
+	"Group base has 1 roots: base_joint \n"
+	"The state components for this group are: 0 1 2 \n";
+    
+    urdf::Model urdfModel;
+    urdfModel.initString(MODEL1);
+    
+    std::map < std::string, std::vector<std::string> > groups;
+    groups["base"].push_back("base_joint");
 
+    planning_models::KinematicModel *model = new planning_models::KinematicModel(urdfModel, groups);
+        
+    EXPECT_EQ((unsigned int)3, model->getDimension());
+    
+    model->defaultState();
+    
+    std::vector<const planning_models::KinematicModel::Joint*> joints;
+    model->getJoints(joints);
+    EXPECT_EQ((unsigned int)1, joints.size());
+    EXPECT_EQ((unsigned int)3, joints[0]->usedParams);
+
+    EXPECT_EQ((unsigned int)1, model->getPlanarJoints().size());
+
+    std::stringstream ssi;
+    model->printModelInfo(ssi);
+    EXPECT_TRUE(sameStringIgnoringWS(MODEL1_INFO, ssi.str()));
+        
+    
+    double param[3] = { 10, 8, 0 };
+    model->computeTransforms(param);
+
+    EXPECT_NEAR(9.9, model->getLink("base_link")->globalTrans.getOrigin().x(), 1e-5);
+    EXPECT_NEAR(8.0, model->getLink("base_link")->globalTrans.getOrigin().y(), 1e-5);
+    EXPECT_NEAR(0.0, model->getLink("base_link")->globalTrans.getOrigin().z(), 1e-5);
+    EXPECT_NEAR(0.0, model->getLink("base_link")->globalTrans.getRotation().x(), 1e-5);
+    EXPECT_NEAR(0.0, model->getLink("base_link")->globalTrans.getRotation().y(), 1e-5);
+    EXPECT_NEAR(-0.479426, model->getLink("base_link")->globalTrans.getRotation().z(), 1e-5);
+    EXPECT_NEAR(0.877583, model->getLink("base_link")->globalTrans.getRotation().w(), 1e-5);
+    
+    delete model;    
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);


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