Menu
▾
▴
personalrobots-commit
|
From: <rob...@us...> - 2008-07-30 23:07:28
|
Revision: 2349
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2349&view=rev
Author: rob_wheeler
Date: 2008-07-30 23:07:34 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
Move hw_interface to deprecated (eventual move everything to hardware_interface)
Added Paths:
-----------
pkg/trunk/deprecated/
pkg/trunk/deprecated/hw_interface/
Removed Paths:
-------------
pkg/trunk/mechanism/hw_interface/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-07-30 23:12:06
|
Revision: 2350
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2350&view=rev
Author: rob_wheeler
Date: 2008-07-30 23:12:12 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
Move mechanism_control to deprecated/old_mechanism_control
Modified Paths:
--------------
pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherDrive/manifest.xml
pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml
Added Paths:
-----------
pkg/trunk/deprecated/old_mechanism_control/
Removed Paths:
-------------
pkg/trunk/mechanism/mechanism_control/
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml 2008-07-30 23:07:34 UTC (rev 2349)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml 2008-07-30 23:12:12 UTC (rev 2350)
@@ -5,7 +5,7 @@
<author>Rob Wheeler/wh...@wi...</author>
<license>BSD</license>
<depend package="roscpp" />
- <depend package="mechanism_control"/>
+ <depend package="old_mechanism_control"/>
<depend package="ethercat_hardware"/>
<depend package="hw_interface"/>
Modified: pkg/trunk/robot_control_loops/pr2_etherDrive/manifest.xml
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherDrive/manifest.xml 2008-07-30 23:07:34 UTC (rev 2349)
+++ pkg/trunk/robot_control_loops/pr2_etherDrive/manifest.xml 2008-07-30 23:12:12 UTC (rev 2350)
@@ -5,7 +5,7 @@
<author>Jimmy Sastra, Sachin Chitta</author>
<license>BSD</license>
<depend package="roscpp" />
- <depend package="mechanism_control"/>
+ <depend package="old_mechanism_control"/>
<depend package="etherdrive_hardware"/>
<depend package="hw_interface"/>
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml 2008-07-30 23:07:34 UTC (rev 2349)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml 2008-07-30 23:12:12 UTC (rev 2350)
@@ -29,5 +29,5 @@
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
</export>
- <depend package="mechanism_control" />
+ <depend package="old_mechanism_control" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <adv...@us...> - 2008-07-30 23:38:01
|
Revision: 2354
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2354&view=rev
Author: advaitjain
Date: 2008-07-30 23:38:02 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
changed reset_IK_guess message to a service -- RosGazeboNode
Modified Paths:
--------------
pkg/trunk/simulators/rosgazebo/CMakeLists.txt
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Added Paths:
-----------
pkg/trunk/manip/pr2_kinematic_controllers/
pkg/trunk/manip/pr2_kinematic_controllers/CMakeLists.txt
pkg/trunk/manip/pr2_kinematic_controllers/Makefile
pkg/trunk/manip/pr2_kinematic_controllers/bin/
pkg/trunk/manip/pr2_kinematic_controllers/include/
pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml
pkg/trunk/manip/pr2_kinematic_controllers/src/
pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc
pkg/trunk/manip/pr2_kinematic_controllers/srv/
pkg/trunk/manip/pr2_kinematic_controllers/srv/Float32Int32.srv
pkg/trunk/manip/pr2_kinematic_controllers/test/
pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc
pkg/trunk/simulators/rosgazebo/srv/
pkg/trunk/simulators/rosgazebo/srv/VoidVoid.srv
Added: pkg/trunk/manip/pr2_kinematic_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/CMakeLists.txt (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/CMakeLists.txt 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(pr2_kinematic_controllers)
+
+gensrv()
+add_definitions(-Wall)
+include_directories(srv/cpp)
+
+rospack_add_executable(bin/pr2_kinematic_controllers src/pr2_kinematic_controllers.cc)
+rospack_add_executable(bin/test_pr2_kin_con test/test_pr2_kin_con.cc)
+
+
Added: pkg/trunk/manip/pr2_kinematic_controllers/Makefile
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/Makefile (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/Makefile 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,2 @@
+include $(shell rospack find mk)/cmake.mk
+
Added: pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,17 @@
+<package>
+ <description brief='Higher level controllers for kinematics. Sourced from interpolated_kinematic_controller'>
+ </description>
+ <author>Advait Jain</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com</url>
+ <depend package="roscpp"/>
+ <depend package="std_msgs"/>
+ <depend package="pr2_msgs"/>
+ <depend package="pr2Core"/>
+ <depend package="libpr2API"/>
+ <depend package="libKDL"/>
+ <depend package="pr2_gazebo"/>
+ <export>
+ <cpp cflags="-I${prefix}/srv/cpp" />
+ </export>
+</package>
Added: pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,194 @@
+#include <ros/node.h>
+#include <rosthread/mutex.h>
+
+#include <std_msgs/Empty.h>
+#include <std_msgs/PR2Arm.h>
+#include <std_msgs/Float64.h>
+#include <pr2_msgs/EndEffectorState.h>
+
+#include <pr2_kinematic_controllers/Float32Int32.h>
+
+#include <libpr2API/pr2API.h>
+
+#include <kdl/rotational_interpolation_sa.hpp>
+#include <rosgazebo/VoidVoid.h>
+
+
+using namespace std_msgs;
+using namespace PR2;
+using namespace KDL;
+
+
+class Pr2KinematicControllers : public ros::node
+{
+ public:
+
+ Pr2KinematicControllers(void) : ros::node("pr2_kinematic_controller")
+ {
+ step_size = 0.05;
+ wait_time = 1.;
+ advertise<pr2_msgs::EndEffectorState>("cmd_leftarm_cartesian");
+ advertise<pr2_msgs::EndEffectorState>("cmd_rightarm_cartesian");
+// advertise<std_msgs::Empty>("reset_IK_guess"); // to tell RosGazeboNode to make q_IK_guess = current manipulator config.
+
+ subscribe("right_pr2arm_set_end_effector", _rightEndEffectorGoal, &Pr2KinematicControllers::setRightEndEffector);
+ subscribe("left_pr2arm_set_end_effector", _leftEndEffectorGoal, &Pr2KinematicControllers::setLeftEndEffector);
+ subscribe("left_pr2arm_pos", leftArmPosMsg, &Pr2KinematicControllers::currentLeftArmPos);
+ subscribe("right_pr2arm_pos", rightArmPosMsg, &Pr2KinematicControllers::currentRightArmPos);
+
+ subscribe("interpolate_step_size", float64_msg, &Pr2KinematicControllers::setStepSize);
+ subscribe("interpolate_wait_time", float64_msg, &Pr2KinematicControllers::setWaitTime);
+ }
+
+ void setWaitTime(void)
+ {
+ wait_time = float64_msg.data;
+ }
+
+ void setStepSize(void)
+ {
+ step_size = float64_msg.data;
+ }
+
+ void setRightEndEffector(void)
+ {
+ KDL::Frame f;
+ for(int i = 0; i < 9; i++)
+ f.M.data[i] = _rightEndEffectorGoal.rot[i];
+
+ for(int i = 0; i < 3; i++)
+ f.p.data[i] = _rightEndEffectorGoal.trans[i];
+
+ RunControlLoop(true, f, rightArmPosMsg);
+ }
+
+ void setLeftEndEffector(void)
+ {
+ KDL::Frame f;
+ for(int i = 0; i < 9; i++)
+ f.M.data[i] = _leftEndEffectorGoal.rot[i];
+
+ for(int i = 0; i < 3; i++)
+ f.p.data[i] = _leftEndEffectorGoal.trans[i];
+
+ RunControlLoop(false, f, leftArmPosMsg);
+ }
+
+
+ void currentLeftArmPos(void)
+ {
+ // don't need to do anything -- we already have the data
+ }
+
+ void currentRightArmPos(void)
+ {
+ // don't need to do anything -- we already have the data
+ }
+
+ void publishFrame(bool isRightArm, const Frame& f)
+ {
+ pr2_msgs::EndEffectorState efs;
+ efs.set_rot_size(9);
+ efs.set_trans_size(3);
+ std::cout << "Publishing rot ";
+ for(int i = 0; i < 9; i++)
+ {
+ efs.rot[i] = f.M.data[i];
+ std::cout << efs.rot[i] << " ";
+ }
+ std::cout << " trans ";
+ for(int i = 0; i < 3; i++)
+ {
+ efs.trans[i] = f.p.data[i];
+ std::cout << efs.trans[i] << " ";
+ }
+ std::cout << std::endl;
+ if(isRightArm)
+ publish("cmd_rightarm_cartesian",efs);
+ else
+ publish("cmd_leftarm_cartesian",efs);
+ }
+
+ void RunControlLoop(bool isRightArm, const Frame& r, const std_msgs::PR2Arm& arm)
+ {
+
+ std_msgs::Empty emp;
+ rosgazebo::VoidVoid::request req;
+ rosgazebo::VoidVoid::response res;
+// publish("reset_IK_guess",emp);
+ if (ros::service::call("reset_IK_guess", req, res))
+ printf("Success!\n");
+ else
+ {
+ printf("reset_IK_guess service failed.\nExiting..\n");
+ exit(0);
+ }
+
+// if (ros::service::call("reset_IK_guess", req, res)==false)
+// {
+// printf("reset_IK_guess service failed.\nExiting..\n");
+// exit(0);
+// }
+
+
+ PR2_kinematics pr2_kin;
+ JntArray q = JntArray(pr2_kin.nJnts);
+
+ q(0) = arm.turretAngle;
+ q(1) = arm.shoulderLiftAngle;
+ q(2) = arm.upperarmRollAngle;
+ q(3) = arm.elbowAngle;
+ q(4) = arm.forearmRollAngle;
+ q(5) = arm.wristPitchAngle;
+ q(6) = arm.wristRollAngle;
+
+ Frame f;
+ pr2_kin.FK(q,f);
+ Vector start = f.p;
+ Vector move = r.p-start;
+ double dist = move.Norm();
+ move = move/dist;
+
+ std::cout << "Starting trans " << f.p.data[0] << " " << f.p.data[1] << " " << f.p.data[2] << std::endl;
+
+ RotationalInterpolation_SingleAxis rotInterpolater;
+ rotInterpolater.SetStartEnd(f.M, r.M);
+ double total_angle = rotInterpolater.Angle();
+ // printf("Angle: %f\n", rotInterpolater.Angle());
+
+ Vector target;
+ int nSteps = (int)(dist/step_size);
+ double angle_step = total_angle/nSteps;
+ for(int i=0;i<nSteps;i++)
+ {
+ f.p = start+(i+1)*move*step_size;
+ f.M = rotInterpolater.Pos(angle_step*(i+1));
+ std::cout << "Starting trans " << f.p.data[0] << " " << f.p.data[1] << " " << f.p.data[2] << std::endl;
+ publishFrame(isRightArm, f);
+ usleep(wait_time*1e6);
+ }
+ f.p = r.p;
+ f.M = r.M;
+ publishFrame(isRightArm, f);
+ }
+
+ private:
+
+ pr2_msgs::EndEffectorState _leftEndEffectorGoal;
+ pr2_msgs::EndEffectorState _rightEndEffectorGoal;
+ std_msgs::PR2Arm leftArmPosMsg, rightArmPosMsg;
+ double step_size;
+ double wait_time;
+ std_msgs::Float64 float64_msg;
+
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ Pr2KinematicControllers easy;
+ easy.spin();
+ easy.shutdown();
+ return 0;
+}
+
Added: pkg/trunk/manip/pr2_kinematic_controllers/srv/Float32Int32.srv
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/srv/Float32Int32.srv (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/srv/Float32Int32.srv 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,3 @@
+float32 f
+---
+int32 i
Added: pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc (rev 0)
+++ pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,47 @@
+#include <ros/node.h>
+#include <libpr2API/pr2API.h>
+#include <pr2_msgs/EndEffectorState.h>
+
+using namespace KDL;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ ros::node mynode("test_inter+kin_con");
+ mynode.advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
+
+ //In shoulder frame x 0.562689
+ //In shoulder frame y -0.367447
+ //In shoulder frame z -0.369594
+ Rotation r = Rotation::RotZ(DTOR(0));
+ Vector v(.562689,-.367447,-.369594);
+
+ std::cout << " rot: " << std::endl;
+ std::cout << r.data[0] << std::endl;
+ std::cout << r.data[1] << std::endl;
+ std::cout << r.data[2] << std::endl;
+ std::cout << r.data[3] << std::endl;
+ std::cout << r.data[4] << std::endl;
+ std::cout << r.data[5] << std::endl;
+ std::cout << r.data[6] << std::endl;
+ std::cout << r.data[7] << std::endl;
+ std::cout << r.data[8] << std::endl;
+
+ sleep(1);
+
+ pr2_msgs::EndEffectorState efs;
+ efs.set_rot_size(9);
+ efs.set_trans_size(3);
+ for(int i = 0; i < 9; i++)
+ efs.rot[i] = r.data[i];
+
+ for(int i = 0; i < 3; i++)
+ efs.trans[i] = v.data[i];
+
+
+ mynode.publish("right_pr2arm_set_end_effector",efs);
+ sleep(1);
+ mynode.shutdown();
+ return 0;
+}
Modified: pkg/trunk/simulators/rosgazebo/CMakeLists.txt
===================================================================
--- pkg/trunk/simulators/rosgazebo/CMakeLists.txt 2008-07-30 23:35:27 UTC (rev 2353)
+++ pkg/trunk/simulators/rosgazebo/CMakeLists.txt 2008-07-30 23:38:02 UTC (rev 2354)
@@ -3,6 +3,8 @@
rospack(rosgazebo)
genmsg()
include_directories(msg/cpp)
+gensrv()
+include_directories(srv/cpp)
add_definitions(-Wall)
rospack_add_library(RosGazeboNode src/RosGazeboNode.cpp)
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-30 23:35:27 UTC (rev 2353)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-30 23:38:02 UTC (rev 2354)
@@ -65,6 +65,9 @@
#include <time.h>
+// service
+#include <rosgazebo/VoidVoid.h>
+
// Our node
class RosGazeboNode : public ros::node
{
@@ -157,7 +160,7 @@
// Message callback which sets pr2_kin.q_IK_guess to the current manipulator configuration.
// July 24, 2008 - Advait - only right arm is supported
- void reset_IK_guess();
+ bool reset_IK_guess(rosgazebo::VoidVoid::request &req, rosgazebo::VoidVoid::response &res);
// laser range data
float ranges[GZ_LASER_MAX_RANGES];
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-30 23:35:27 UTC (rev 2353)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-30 23:38:02 UTC (rev 2354)
@@ -23,6 +23,6 @@
<depend package="gazebo_hardware" />
<depend package="gazebo_sensors" />
<export>
- <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRosGazeboNode"/>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRosGazeboNode"/>
</export>
</package>
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-30 23:35:27 UTC (rev 2353)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-30 23:38:02 UTC (rev 2354)
@@ -126,9 +126,10 @@
this->lock.unlock();
}
-void RosGazeboNode::reset_IK_guess()
+bool RosGazeboNode::reset_IK_guess(rosgazebo::VoidVoid::request &req, rosgazebo::VoidVoid::response &res)
{
this->PR2Copy->GetArmJointPositionCmd(PR2::PR2_RIGHT_ARM, *(this->PR2Copy->pr2_kin.q_IK_guess));
+ return true;
}
void RosGazeboNode::cmd_rightarmcartesianReceived() {
@@ -354,8 +355,10 @@
subscribe("cmd_rightarmconfig", rightarm, &RosGazeboNode::cmd_rightarmconfigReceived);
subscribe("cmd_leftarm_cartesian", cmd_leftarmcartesian, &RosGazeboNode::cmd_leftarmcartesianReceived);
subscribe("cmd_rightarm_cartesian", cmd_rightarmcartesian, &RosGazeboNode::cmd_rightarmcartesianReceived);
- subscribe("reset_IK_guess", empty_msg, &RosGazeboNode::reset_IK_guess);
+ //------ services ----------
+ advertise_service("reset_IK_guess", &RosGazeboNode::reset_IK_guess);
+
return(0);
}
Added: pkg/trunk/simulators/rosgazebo/srv/VoidVoid.srv
===================================================================
--- pkg/trunk/simulators/rosgazebo/srv/VoidVoid.srv (rev 0)
+++ pkg/trunk/simulators/rosgazebo/srv/VoidVoid.srv 2008-07-30 23:38:02 UTC (rev 2354)
@@ -0,0 +1,3 @@
+int32 i
+---
+int32 i
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-07-31 09:24:27
|
Revision: 2363
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2363&view=rev
Author: rob_wheeler
Date: 2008-07-31 09:24:29 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
- Make a version of pr2_etherCAT that uses Stu's new mechanism_control.
- Deprecate robot_mechanism_model in favor of mechanism_model
- Deprecate genericControllers in favor of generic_controllers
- Bring various bits of code up to Brian's CppStyleGuide standards
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/manifest.xml
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Added Paths:
-----------
pkg/trunk/deprecated/genericControllers/
pkg/trunk/deprecated/robot_mechanism_model/
pkg/trunk/mechanism/mechanism_control/
pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/Makefile
pkg/trunk/mechanism/mechanism_control/include/
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control/src/
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/Makefile
pkg/trunk/mechanism/mechanism_model/include/
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
pkg/trunk/mechanism/mechanism_model/src/
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
Removed Paths:
-------------
pkg/trunk/controllers/genericControllers/
pkg/trunk/robot_models/robot_mechanism_model/
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -35,8 +35,10 @@
#ifndef ETHERCAT_HARDWARE_H
#define ETHERCAT_HARDWARE_H
-#include <hw_interface/hardware_interface.h>
+#include <tinyxml/tinyxml.h>
+#include <hardware_interface/hardware_interface.h>
+
#include <al/ethercat_AL.h>
#include <al/ethercat_master.h>
#include <al/ethercat_slave_handler.h>
@@ -64,15 +66,15 @@
/*!
* \brief Initialize the EtherCAT Master Library.
*/
- void init(char *interface, char *configuration);
+ void init(char *interface, TiXmlElement *config);
- HardwareInterface *hw;
+ HardwareInterface *hw_;
private:
- struct netif *ni;
+ struct netif *ni_;
- EtherCAT_AL *al;
- EtherCAT_Master *em;
+ EtherCAT_AL *al_;
+ EtherCAT_Master *em_;
MotorControlBoard *configSlave(EtherCAT_SlaveHandler *sh);
MotorControlBoard **slaves;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/motor_control_board.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -40,7 +40,7 @@
#include <ethercat/ethercat_defs.h>
#include <al/ethercat_slave_handler.h>
-#include <hw_interface/hardware_interface.h>
+#include <hardware_interface/hardware_interface.h>
using namespace std;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -39,35 +39,35 @@
struct WG05Status
{
- uint16_t deviceTypeId;
- uint16_t deviceRev;
- uint8_t mode;
- uint8_t digitalOut;
- uint16_t pwmDuty;
- uint16_t programmedCurrent;
- uint8_t currentLoopKp;
- uint8_t currentLoopKi;
- uint16_t measuredCurrent;
- uint16_t pad1;
- uint32_t timestamp;
- uint32_t encoderCount;
- uint32_t encoderIndexPos;
- uint16_t numEncoderErrors;
- uint8_t encoderStatus;
- uint8_t calibrationReading;
- uint32_t lastCalibrationHighTransition;
- uint32_t lastCalibrationLowTransition;
- uint16_t supplyVoltage;
- uint16_t motorVoltage;
- uint16_t boardTemperature;
- uint16_t bridgeTemperature;
- uint8_t pdoCommandIrqCount;
- uint8_t mbxCommandIrqCount;
- uint16_t packetCount;
- uint16_t pdiTimeoutErrorCount;
- uint16_t pdiChecksumErrorCount;
- uint8_t pad2;
- uint8_t checksum;
+ uint16_t device_type_id_;
+ uint16_t device_rev_;
+ uint8_t mode_;
+ uint8_t digital_out_;
+ uint16_t pwm_duty_;
+ uint16_t programmed_current_;
+ uint8_t current_loop_kp_;
+ uint8_t current_loop_ki_;
+ uint16_t measured_current_;
+ uint16_t pad1_;
+ uint32_t timestamp_;
+ uint32_t encoder_count_;
+ uint32_t encoder_index_pos_;
+ uint16_t num_encoder_errors_;
+ uint8_t encoder_status_;
+ uint8_t calibration_reading_;
+ uint32_t last_calibration_high_transition_;
+ uint32_t last_calibration_low_transition_;
+ uint16_t supply_voltage_;
+ uint16_t motor_voltage_;
+ uint16_t board_temperature_;
+ uint16_t bridge_temperature_;
+ uint8_t pdo_command_irq_count_;
+ uint8_t mbx_command_irq_count_;
+ uint16_t packet_count_;
+ uint16_t pdi_timeout_error_count_;
+ uint16_t pdi_checksum_error_count_;
+ uint8_t pad2_;
+ uint8_t checksum_;
}__attribute__ ((__packed__));
typedef WG05Status WG05Command;
@@ -92,7 +92,7 @@
MotorControlBoard(WG05_PRODUCT_CODE, sizeof(WG05Command), sizeof(WG05Status))
{
}
- void configure(int &startAddress, EtherCAT_SlaveHandler *sh);
+ void configure(int &start_address, EtherCAT_SlaveHandler *sh);
void convertCommand(ActuatorCommand &command, unsigned char *buffer);
void convertState(ActuatorState &state, unsigned char *current_buffer, unsigned char *last_buffer);
bool hasActuator(void)
Modified: pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-07-31 09:24:29 UTC (rev 2363)
@@ -5,8 +5,9 @@
<author>Rob Wheeler (email: wh...@wi...)</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
-<depend package='hw_interface'/>
+<depend package='hardware_interface'/>
<depend package='eml'/>
+<depend package='tinyxml'/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lethercat_hardware -Wl,-rpath,${prefix}/lib"/>
</export>
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -39,15 +39,15 @@
#include <dll/ethercat_dll.h>
EthercatHardware::EthercatHardware() :
- hw(0), ni(0), current_buffer_(0), last_buffer_(0), buffer_size_(0)
+ hw_(0), ni_(0), current_buffer_(0), last_buffer_(0), buffer_size_(0)
{
}
EthercatHardware::~EthercatHardware()
{
- if (ni)
+ if (ni_)
{
- close_socket(ni);
+ close_socket(ni_);
}
if (current_buffer_)
{
@@ -57,50 +57,50 @@
{
delete last_buffer_;
}
- if (hw)
+ if (hw_)
{
- delete hw;
+ delete hw_;
}
}
-void EthercatHardware::init(char *interface, char *configuration)
+void EthercatHardware::init(char *interface, TiXmlElement *configuration)
{
// Initialize network interface
- if ((ni = init_ec(interface)) == NULL)
+ if ((ni_ = init_ec(interface)) == NULL)
{
perror("init_ec");
return;
}
// Initialize Application Layer (AL)
- EtherCAT_DataLinkLayer::instance()->attach(ni);
- if ((al = EtherCAT_AL::instance()) == NULL)
+ EtherCAT_DataLinkLayer::instance()->attach(ni_);
+ if ((al_ = EtherCAT_AL::instance()) == NULL)
{
perror("EtherCAT_AL::instance");
return;
}
- unsigned int numSlaves = al->get_num_slaves();
- if (numSlaves == 0)
+ unsigned int num_slaves = al_->get_num_slaves();
+ if (num_slaves == 0)
{
perror("Can't locate any slaves");
return;
}
// Initialize Master
- if ((em = EtherCAT_Master::instance()) == NULL)
+ if ((em_ = EtherCAT_Master::instance()) == NULL)
{
perror("EtherCAT_Master::instance");
return;
}
- slaves = new MotorControlBoard*[numSlaves];
+ slaves = new MotorControlBoard*[num_slaves];
unsigned int num_actuators = 0;
- for (unsigned int slave = 0; slave < numSlaves; ++slave)
+ for (unsigned int slave = 0; slave < num_slaves; ++slave)
{
EC_FixedStationAddress fsa(slave + 1);
- EtherCAT_SlaveHandler *sh = em->get_slave_handler(fsa);
+ EtherCAT_SlaveHandler *sh = em_->get_slave_handler(fsa);
if (sh == NULL)
{
perror("get_slave_handler");
@@ -123,7 +123,7 @@
// Determine configuration from XML file 'configuration'
// Create HardwareInterface
- hw = new HardwareInterface(num_actuators);
+ hw_ = new HardwareInterface(num_actuators);
}
void EthercatHardware::update()
@@ -132,21 +132,21 @@
// Convert HW Interface commands to MCB-specific buffers
current = current_buffer_;
- for (int i = 0; i < hw->numActuators; ++i)
+ for (int i = 0; i < hw_->num_actuators_; ++i)
{
- slaves[i]->convertCommand(hw->actuator[i].command, current);
+ slaves[i]->convertCommand(hw_->actuators_[i].command_, current);
current += slaves[i]->commandSize + slaves[i]->statusSize;
}
// Transmit process data
- em->txandrx_PD(buffer_size_, current_buffer_);
+ em_->txandrx_PD(buffer_size_, current_buffer_);
// Convert status back to HW Interface
current = current_buffer_;
last = last_buffer_;
- for (int i = 0; i < hw->numActuators; ++i)
+ for (int i = 0; i < hw_->num_actuators_; ++i)
{
- slaves[i]->convertState(hw->actuator[i].state, current, last);
+ slaves[i]->convertState(hw_->actuators_[i].state_, current, last);
current += slaves[i]->commandSize + slaves[i]->statusSize;
last += slaves[i]->commandSize + slaves[i]->statusSize;
}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/mk1001.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -101,8 +101,8 @@
c.config = 1;
c.i_offset = 0;//-2000;
- c.i_desire = command.current;
- c.mode = command.enable ? MODE_PWM : MODE_OFF;
+ c.i_desire = command.current_;
+ c.mode = command.enable_ ? MODE_PWM : MODE_OFF;
memcpy(buffer + sizeof(MK1001Status), &c, sizeof(c));
}
@@ -114,10 +114,10 @@
memcpy(&s, buffer, sizeof(s));
memcpy(&c, buffer + sizeof(s), sizeof(c));
- state.isEnabled = c.mode != MODE_OFF;
- state.timestamp = s.timestamp;
- state.encoderCount = s.qei_pos;
- state.encoderVelocity = s.qei_velocity;
- state.lastMeasuredCurrent = s.adc_current;
+ state.is_enabled_ = c.mode != MODE_OFF;
+ state.timestamp_ = s.timestamp;
+ state.encoder_count_ = s.qei_pos;
+ state.encoder_velocity_ = s.qei_velocity;
+ state.last_measured_current_ = s.adc_current;
}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-07-31 00:04:10 UTC (rev 2362)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -93,10 +93,10 @@
memset(&c, 0, sizeof(c));
- c.currentLoopKi = Ki;
+ c.current_loop_ki_ = Ki;
- c.programmedCurrent = CURRENT_FACTOR * command.current;
- c.mode = command.enable ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
+ c.programmed_current_ = CURRENT_FACTOR * command.current_;
+ c.mode_ = command.enable_ ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
memcpy(buffer + sizeof(WG05Status), &c, sizeof(c));
}
@@ -112,44 +112,23 @@
memcpy(&last_status, last_buffer, sizeof(last_status));
memcpy(&last_command, last_buffer + sizeof(last_status), sizeof(last_command));
- state.encoderCount = current_status.encoderCount;
- state.encoderVelocity = double(int(current_status.encoderCount - last_status.encoderCount)) / (current_status.timestamp - last_status.timestamp) * 1e+6;
- state.calibrationReading = current_status.calibrationReading;
- state.lastCalibrationHighTransition = current_status.lastCalibrationHighTransition;
- state.lastCalibrationLowTransition = current_status.lastCalibrationLowTransition;
- state.isEnabled = current_status.mode != MODE_OFF;
- state.runStopHit = (current_status.mode & MODE_UNDERVOLTAGE) != 0;
+ state.encoder_count_ = current_status.encoder_count_;
+ state.encoder_velocity_ = double(int(current_status.encoder_count_ - last_status.encoder_count_)) / (current_status.timestamp_ - last_status.timestamp_) * 1e+6;
+ state.calibration_reading_ = current_status.calibration_reading_;
+ state.last_calibration_high_transition_ = current_status.last_calibration_high_transition_;
+ state.last_calibration_low_transition_ = current_status.last_calibration_low_transition_;
+ state.is_enabled_ = current_status.mode_ != MODE_OFF;
+ state.run_stop_hit_ = (current_status.mode_ & MODE_UNDERVOLTAGE) != 0;
- state.lastRequestedCurrent = current_command.programmedCurrent / CURRENT_FACTOR; // Should be actual request, before safety
- state.lastCommandedCurrent = current_status.programmedCurrent / CURRENT_FACTOR;
- state.lastMeasuredCurrent = current_status.measuredCurrent / CURRENT_FACTOR;
+ state.last_requested_current_ = current_command.programmed_current_ / CURRENT_FACTOR; // TODO should be pre-safety code value
+ state.last_commanded_current_ = current_status.programmed_current_ / CURRENT_FACTOR;
+ state.last_measured_current_ = current_status.measured_current_ / CURRENT_FACTOR;
- state.numEncoderErrors = current_status.numEncoderErrors;
- state.numCommunicationErrors = current_status.pdiTimeoutErrorCount + current_status.pdiChecksumErrorCount;
+ state.num_encoder_errors_ = current_status.num_encoder_errors_;
+ state.num_communication_errors_ = current_status.pdi_timeout_error_count_ + current_status.pdi_checksum_error_count_;
- state.motorVoltage = current_status.motorVoltage;
-#if 1
-{
-static int times = 0;
+ state.motor_voltage_ = current_status.motor_voltage_;
-if (++times % 2000 == 0) {
- printf("-----------------------------------------------------\n");
- printf("timestamp = %#08x\n", current_status.timestamp);
- printf("encoderCount = %#08x\n", current_status.encoderCount);
- printf("last timestamp = %#08x\n", last_status.timestamp);
- printf("last encoderCount = %#08x\n", last_status.encoderCount);
- printf("delta encoderCount = %#08x %f\n", current_status.encoderCount - last_status.encoderCount, double(int(current_status.encoderCount - last_status.encoderCount)));
- printf("delta timestamp = %#08x %f\n", current_status.timestamp - last_status.timestamp, double(current_status.timestamp - last_status.timestamp));
- printf("encoderVelocity = %f\n", state.encoderVelocity);
- printf("programmedCurrent = %#08x\n", current_status.programmedCurrent);
- printf("measuredCurrent = %#08x\n", current_status.measuredCurrent);
- printf("mode = %#08x\n", current_status.mode);
- printf("Kp = %#08x\n", int(current_status.currentLoopKp));
- printf("Ki = %#08x\n", int(current_status.currentLoopKi));
}
-}
-#endif
-}
-
Added: pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_control/CMakeLists.txt (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/CMakeLists.txt 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(mechanism_control)
+rospack_add_library(mechanism_control src/mechanism_control.cpp)
+
Added: pkg/trunk/mechanism/mechanism_control/Makefile
===================================================================
--- pkg/trunk/mechanism/mechanism_control/Makefile (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/Makefile 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,65 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+#ifndef MECHANISM_CONTROL_H
+#define MECHANISM_CONTROL_H
+
+#include <map>
+#include <string>
+#include <vector>
+#include <tinyxml/tinyxml.h>
+#include <hardware_interface/hardware_interface.h>
+#include <mechanism_model/robot.h>
+#include <rosthread/mutex.h>
+#include <generic_controllers/controller.h>
+
+typedef controller::Controller* (*ControllerAllocator)();
+
+class MechanismControl {
+public:
+ MechanismControl(HardwareInterface *hw);
+ ~MechanismControl();
+
+ bool init(TiXmlElement* config);
+ void update(); //Must be realtime safe
+
+ bool registerActuator(const std::string &name, int index);
+ void registerControllerType(const std::string& type, ControllerAllocator f);
+ bool spawnController(const char* type, TiXmlElement* config);
+
+private:
+ bool initialized_;
+ mechanism::Robot model_;
+ HardwareInterface *hw_;
+
+ std::map<std::string, ControllerAllocator> controller_library_;
+ const static int MAX_NUM_CONTROLLERS = 100;
+ ros::thread::mutex controllers_mutex_;
+ controller::Controller* controllers_[MAX_NUM_CONTROLLERS];
+};
+
+#endif /* MECHANISM_CONTROL_H */
Added: pkg/trunk/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control/manifest.xml (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,16 @@
+<package>
+<description brief="Mechanism control">
+</description>
+<author>Eric Berger be...@wi...</author>
+<license>BSD</license>
+<url>http://pr.willowgarage.com</url>
+<depend package="rosthread"/>
+<depend package="hardware_interface"/>
+<depend package="tinyxml"/>
+<depend package="mechanism_model"/>
+<depend package="generic_controllers"/>
+<export>
+ <cpp cflags='-I${prefix}/include' lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control'/>
+</export>
+</package>
+
Added: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,172 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Willow Garage, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Willow Garage, Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#include "mechanism_control/mechanism_control.h"
+
+using namespace mechanism;
+
+MechanismControl::MechanismControl(HardwareInterface *hw)
+ : initialized_(0), model_((char*)"robot"), hw_(hw)
+{
+ memset(controllers_, 0, MAX_NUM_CONTROLLERS*sizeof(void*));
+ model_.hw_ = hw;
+}
+
+MechanismControl::~MechanismControl() {
+}
+
+bool MechanismControl::registerActuator(const std::string &name, int index)
+{
+ if (initialized_)
+ return false;
+
+ model_.actuators_lookup_.insert(Robot::IndexMap::value_type(name, index));
+
+ return true;
+}
+
+bool MechanismControl::init(TiXmlElement* config)
+{
+ bool successful = true;
+
+ TiXmlElement *elt;
+
+ // Constructs the joints
+ std::map<std::string, Joint*> joint_map;
+ for (elt = config->FirstChildElement("joint"); elt; elt = elt->NextSiblingElement("joint"))
+ {
+ Joint *j = new Joint;
+ model_.joints_.push_back(j);
+ model_.joints_lookup_.insert(
+ Robot::IndexMap::value_type(elt->Attribute("name"), model_.joints_.size() - 1));
+ j->joint_limit_min_ = atof(elt->FirstChildElement("limitMin")->GetText());
+ j->joint_limit_max_ = atof(elt->FirstChildElement("limitMax")->GetText());
+ j->effort_limit_ = atof(elt->FirstChildElement("effortLimit")->GetText());
+ j->velocity_limit_ = atof(elt->FirstChildElement("velocityLimit")->GetText());
+ }
+
+ // Constructs the transmissions
+ elt = config->FirstChildElement("transmission");
+ for (; elt; elt = elt->NextSiblingElement("transmission"))
+ {
+ if (0 == strcmp("SimpleTransmission", elt->Attribute("type")))
+ {
+ // Looks up the joint and the actuator used by the transmission.
+ Robot::IndexMap::iterator joint_it =
+ model_.joints_lookup_.find(elt->FirstChildElement("joint")->Attribute("name"));
+ Robot::IndexMap::iterator actuator_it =
+ model_.actuators_lookup_.find(elt->FirstChildElement("actuator")->Attribute("name"));
+ if (joint_it == model_.joints_lookup_.end())
+ {
+ // TODO: report: The joint was not declared in the XML file
+ continue;
+ }
+ if (actuator_it == model_.actuators_lookup_.end())
+ {
+ // TODO: report: The actuator was not registered with mechanism control.
+ continue;
+ }
+
+ Transmission *tr = new SimpleTransmission
+ (model_.joints_[joint_it->second], &hw_->actuators_[actuator_it->second],
+ atof(elt->FirstChildElement("mechanicalReduction")->Value()),
+ atof(elt->FirstChildElement("motorTorqueConstant")->Value()),
+ atof(elt->FirstChildElement("pulsesPerRevolution")->Value()));
+ model_.transmissions_.push_back(tr);
+ }
+ else
+ {
+ // TODO: report: Unknown transmission type
+ successful = false;
+ }
+ }
+
+ initialized_ = true;
+ return successful;
+}
+
+// Must be realtime safe.
+void MechanismControl::update() {
+
+ // Propagates through the robot model.
+ for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
+ model_.transmissions_[i]->propagatePosition();
+
+ // TODO: update KDL model with new joint position/velocities
+
+ //update all controllers
+ for(int i = 0; i < MAX_NUM_CONTROLLERS; ++i){
+ if(controllers_[i] != NULL)
+ controllers_[i]->update();
+ }
+
+ // Performs safety checks on the commands.
+ for (unsigned int i = 0; i < model_.joints_.size(); ++i)
+ model_.joints_[i]->enforceLimits();
+
+ // Propagates commands back into the actuators.
+ for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
+ model_.transmissions_[i]->propagateEffort();
+}
+
+
+
+void MechanismControl::registerControllerType(const std::string& type, ControllerAllocator f){
+ controller_library_.insert(std::pair<std::string,ControllerAllocator>(type, f));
+}
+
+bool MechanismControl::spawnController(const char *type, TiXmlElement *config)
+{
+ controller::Controller *c;
+ ControllerAllocator f = controller_library_[type];
+ c = f();
+ c->initXml(&model_, config);
+
+ //At this point, the controller is fully initialized and has created the ROS interface, etc.
+
+ //Add controller to list of controllers in realtime-safe manner;
+ controllers_mutex_.lock(); //This lock is only to prevent us from other non-realtime threads. The realtime thread may be spinning through the list of controllers while we are in here, so we need to keep that list always in a valid state. This is why we fully allocate and set up the controller before adding it into the list of active controllers.
+ bool spot_found = false;
+ for(int i = 0; i < MAX_NUM_CONTROLLERS; i++){
+ if(controllers_[i] == NULL)
+ {
+ spot_found = true;
+ controllers_[i] = c;
+ break;
+ }
+ }
+ controllers_mutex_.unlock();
+
+ if (!spot_found)
+ {
+ delete c;
+ return false;
+ }
+
+ return true;
+}
Added: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(mechanism_model)
+rospack_add_library(mechanism_model src/simple_transmission.cpp)
Added: pkg/trunk/mechanism/mechanism_model/Makefile
===================================================================
--- pkg/trunk/mechanism/mechanism_model/Makefile (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/Makefile 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-07-31 09:24:29 UTC (rev 2363)
@@ -0,0 +1,77 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary...
[truncated message content] |
|
From: <is...@us...> - 2008-07-31 16:46:36
|
Revision: 2369
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2369&view=rev
Author: isucan
Date: 2008-07-31 16:46:44 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
deprecating old planning framework (based on MPK)
Added Paths:
-----------
pkg/trunk/deprecated/old_motion_planning/
pkg/trunk/deprecated/old_motion_planning/sbl_pr2arm_mpk/
pkg/trunk/deprecated/old_motion_planning/test_kinematic_planning/
pkg/trunk/deprecated/old_motion_planning/test_test_kinematic_planning/
Removed Paths:
-------------
pkg/trunk/motion_planning/sbl_pr2arm_mpk/
pkg/trunk/motion_planning/test_kinematic_planning/
pkg/trunk/motion_planning/test_test_kinematic_planning/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-07-31 20:17:56
|
Revision: 2383
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2383&view=rev
Author: tfoote
Date: 2008-07-31 20:18:03 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
This is a merge of my work offline last night.
There is now a laser projection class being used in wavefront player.
r5861@zug: tfoote | 2008-07-30 18:03:19 -0700
creating a copy of pkg trunk for offline use
r5862@zug: tfoote | 2008-07-30 18:04:24 -0700
adding in projection functions
r5863@zug: tfoote | 2008-07-30 18:32:07 -0700
the projection is compiling with caching
r5864@zug: tfoote | 2008-07-30 18:59:23 -0700
adding destructor with deallocation and lots of comments
r5865@zug: tfoote | 2008-07-31 10:25:53 -0700
successful test using wavefront
Modified Paths:
--------------
pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
pkg/trunk/nav/wavefront_player/manifest.xml
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
Property Changed:
----------------
pkg/trunk/
Property changes on: pkg/trunk
___________________________________________________________________
Added: svk:merge
+ 920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865
Modified: pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp 2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp 2008-07-31 20:18:03 UTC (rev 2383)
@@ -304,9 +304,9 @@
private:
- PointCloudFloat32 inputCloud;
- PointCloudFloat32 toProcess;
- std::deque<PointCloudFloat32*> currentWorld;
+ PointCloudFloat32 inputCloud; //Buffer for recieving cloud
+ PointCloudFloat32 toProcess; //Buffer (size 1) for incoming cloud
+ std::deque<PointCloudFloat32*> currentWorld;// Pointers to saved clouds
rosTFClient tf;
double maxPublishFrequency;
Modified: pkg/trunk/nav/wavefront_player/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront_player/manifest.xml 2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/nav/wavefront_player/manifest.xml 2008-07-31 20:18:03 UTC (rev 2383)
@@ -4,6 +4,7 @@
<license>BSD</license>
<depend package="roscpp" />
<depend package="player" />
+ <depend package="laser_scan_utils" />
<depend package="std_srvs" />
<depend package="rosTF" />
<export>
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-07-31 20:18:03 UTC (rev 2383)
@@ -130,6 +130,9 @@
// For transform support
#include <rosTF/rosTF.h>
+//Laser projection
+#include "laser_scan_utils/laser_scan.h"
+
// For time support
#include <ros/time.h>
@@ -218,7 +221,8 @@
// Internal helpers
void sendVelCmd(double vx, double vy, double vth);
-
+
+ laser_scan::LaserProjection projector_;
public:
// Transform client
rosTFClient tf;
@@ -426,8 +430,12 @@
continue;
}
+
// Assemble a point cloud, in the laser's frame
std_msgs::PointCloudFloat32 local_cloud;
+ projector_.projectLaser(*it, local_cloud);
+
+ /* Replaced by above!!
local_cloud.header.frame_id = it->header.frame_id;
local_cloud.header.stamp = it->header.stamp;
local_cloud.set_pts_size(it->get_ranges_size());
@@ -455,7 +463,7 @@
// Resize, because we may have thrown out some points
local_cloud.set_pts_size(cnt);
-
+*/
// Convert to a point cloud in the map frame
std_msgs::PointCloudFloat32 global_cloud;
Modified: pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
===================================================================
--- pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h 2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h 2008-07-31 20:18:03 UTC (rev 2383)
@@ -27,30 +27,56 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
-#ifndef LASER_SCAN_H
-#define LASER_SCAN_H
+#ifndef LASER_SCAN_UTILS_LASERSCAN_H
+#define LASER_SCAN_UTILS_LASERSCAN_H
+#include <map>
#include <iostream>
+#include <sstream>
+
#include <newmat10/newmat.h>
#include <newmat10/newmatio.h>
#include <newmat10/newmatap.h>
#include "math_utils/math_utils.h"
#include "std_msgs/LaserScan.h"
+#include "std_msgs/PointCloudFloat32.h"
/* \mainpage
* This is a class for laser scan utilities.
- * The first goal will be to project laser scans into point clouds efficiently.
+ * \todo The first goal will be to project laser scans into point clouds efficiently.
* The second goal is to provide median filtering.
+ * \todo Other potential additions are upsampling and downsampling algorithms for the scans.
*/
namespace laser_scan{
-
- /** \brief Project Laser Scan
- * This will project a laser scan from a linear array into a 3D point cloud
+ /** \brief A Class to Project Laser Scan
+ * This class will project laser scans into point clouds, and caches unit vectors
+ * between runs so as not to need to recalculate.
*/
- // void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out);
+ class LaserProjection
+ {
+ public:
+ /** \brief Destructor to deallocate stored unit vectors */
+ ~LaserProjection();
+
+ /** \brief Project Laser Scan
+ * This will project a laser scan from a linear array into a 3D point cloud
+ */
+ void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out);
+
+ /** \brief Return the unit vectors for this configuration
+ * Return the unit vectors for this configuration.
+ * if they have not been calculated yet, calculate them and store them
+ * Otherwise it will return them from memory. */
+ NEWMAT::Matrix& getUnitVectors(float angle_max, float angle_min, float angle_increment);
+ private:
+ ///The map of pointers to stored values
+ std::map<std::string,NEWMAT::Matrix*> unit_vector_map_;
+
+ };
+ /** \brief A class to provide median filtering of laser scans */
class LaserMedianFilter
{
public:
@@ -58,7 +84,8 @@
/** \brief Constructor
* \param averaging_length How many scans to average over.
- * \param Whether to downsample and return or compute a rolling median over the last n scans
+ * \param num_ranges Whether to downsample and return or compute a rolling median over the last n scans
+ * \param mode What mode to operate in Trailing or Downsampling (Effectively changes returning true every time or every 3)
*/
LaserMedianFilter(unsigned int averaging_length, unsigned int num_ranges, MedianMode_t mode = MEDIAN_DOWNSAMPLE);
/** \brief Add a scan to the filter
@@ -66,22 +93,22 @@
* return whether there is a new output to get */
bool addScan(const std_msgs::LaserScan& scan_in);
/** \brief get the Filtered results
- * \param The scan to fill with the median results */
+ * \param scan_result The scan to fill with the median results */
void getMedian(std_msgs::LaserScan& scan_result);
-
-
+
+
private:
- unsigned int current_packet_num_;
- NEWMAT::Matrix range_data_;
- NEWMAT::Matrix intensity_data_;
- unsigned int filter_length_;
- unsigned int num_ranges_;
- MedianMode_t mode_;
-
+ unsigned int current_packet_num_; ///The number of scans recieved
+ NEWMAT::Matrix range_data_; ///Storage for range_data
+ NEWMAT::Matrix intensity_data_; ///Storage for intensity data
+ unsigned int filter_length_; ///How many scans to average over
+ unsigned int num_ranges_; /// How many data point are in each row
+ MedianMode_t mode_; ///Whether to return true every time or once every 3
+
std_msgs::LaserScan temp_scan_; /** \todo cache only shallow info not full scan */
};
}
-#endif //LASER_SCAN_H
+#endif //LASER_SCAN_UTILS_LASERSCAN_H
Modified: pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-07-31 20:18:03 UTC (rev 2383)
@@ -30,9 +30,86 @@
#include "laser_scan_utils/laser_scan.h"
-/** \todo add other channel pass throughs */
namespace laser_scan{
+
+ void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out)
+ {
+ NEWMAT::Matrix ranges(2, scan_in.ranges_size);
+ double * matPointer = ranges.Store();
+ // Fill the ranges matrix
+ for (unsigned int index = 0; index < scan_in.ranges_size; index++)
+ {
+ matPointer[index] = (double) scan_in.ranges[index];
+ matPointer[index+scan_in.ranges_size] = (double) scan_in.ranges[index];
+ }
+
+ //Do the projection
+ NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
+
+
+ //Stuff the output cloud
+ cloud_out.header = scan_in.header;
+ cloud_out.set_pts_size(scan_in.ranges_size);
+
+ double* outputMat = output.Store();
+
+ unsigned int count = 0;
+ for (unsigned int index = 0; index< scan_in.ranges_size; index++)
+ {
+ if (matPointer[index] <= scan_in.range_max
+ && matPointer[index] >= scan_in.range_min) //only valid
+ {
+ cloud_out.pts[count].x = outputMat[index];
+ cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+ cloud_out.pts[count].z = 0.0;
+ count++;
+ }
+ }
+
+ //downsize if necessary
+ cloud_out.set_pts_size(count);
+
+ /** \todo Add intensity to channel 1 */
+
+ };
+
+ NEWMAT::Matrix& LaserProjection::getUnitVectors(float angle_min, float angle_max, float angle_increment)
+ {
+ //construct string for lookup in the map
+ std::stringstream anglestring;
+ anglestring <<angle_min<<","<<angle_max<<","<<angle_increment;
+ std::map<string, NEWMAT::Matrix*>::iterator it;
+ it = unit_vector_map_.find(anglestring.str());
+ //check the map for presense
+ if (it != unit_vector_map_.end())
+ return *((*it).second); //if present return
+ //else calculate
+ unsigned int length = (unsigned int) ((angle_max - angle_min)/angle_increment) + 1;//fixme assuming it's calculated right
+ NEWMAT::Matrix * tempPtr = new NEWMAT::Matrix(2,length);
+ for (unsigned int index = 0;index < length; index++)
+ {
+ (*tempPtr)(1,index+1) = cos(angle_min + (double) index * angle_increment);
+ (*tempPtr)(2,index+1) = sin(angle_min + (double) index * angle_increment);
+ }
+ //store
+ unit_vector_map_[anglestring.str()] = tempPtr;
+ //and return
+ return *tempPtr;
+ };
+
+
+ LaserProjection::~LaserProjection()
+ {
+ std::map<string, NEWMAT::Matrix*>::iterator it;
+ it = unit_vector_map_.begin();
+ while (it != unit_vector_map_.end())
+ {
+ delete (*it).second;
+ it++;
+ }
+ };
+
LaserMedianFilter::LaserMedianFilter(unsigned int filter_length, unsigned int num_ranges, MedianMode_t mode):
current_packet_num_(0),
range_data_(filter_length,num_ranges),
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-07-31 21:33:45
|
Revision: 2392
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2392&view=rev
Author: gerkey
Date: 2008-07-31 21:33:53 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
added ubuntu package deps and svn urls to manifests
Modified Paths:
--------------
pkg/trunk/drivers/cam/ptgrey/flea2/manifest.xml
pkg/trunk/visualization/ogre_tools/manifest.xml
pkg/trunk/visualization/pr2_gui/manifest.xml
pkg/trunk/visualization/wx_ros/wx_camera_panel/manifest.xml
pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml
Modified: pkg/trunk/drivers/cam/ptgrey/flea2/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/flea2/manifest.xml 2008-07-31 21:18:15 UTC (rev 2391)
+++ pkg/trunk/drivers/cam/ptgrey/flea2/manifest.xml 2008-07-31 21:33:53 UTC (rev 2392)
@@ -8,4 +8,6 @@
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lflea2 -ldc1394_control -lraw1394"/>
</export>
+ <sysdepend os="Ubuntu" version="7.04" package="libdc1394-13-dev"/>
+ <sysdepend os="Ubuntu" version="8.04.1" package="libdc1394-13-dev"/>
</package>
Modified: pkg/trunk/visualization/ogre_tools/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_tools/manifest.xml 2008-07-31 21:18:15 UTC (rev 2391)
+++ pkg/trunk/visualization/ogre_tools/manifest.xml 2008-07-31 21:33:53 UTC (rev 2392)
@@ -11,5 +11,7 @@
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -logretools"/>
</export>
+ <sysdepend os="Ubuntu" version="7.04" package="libwxgtk2.8-dev"/>
+ <sysdepend os="Ubuntu" version="8.04.1" package="libwxgtk2.8-dev"/>
</package>
Modified: pkg/trunk/visualization/pr2_gui/manifest.xml
===================================================================
--- pkg/trunk/visualization/pr2_gui/manifest.xml 2008-07-31 21:18:15 UTC (rev 2391)
+++ pkg/trunk/visualization/pr2_gui/manifest.xml 2008-07-31 21:33:53 UTC (rev 2392)
@@ -17,5 +17,8 @@
<depend package="wg_robot_description_parser"/>
<export>
</export>
+
+<sysdepend os="Ubuntu" version="7.04" package="libwxgtk2.8-dev"/>
+<sysdepend os="Ubuntu" version="8.04.1" package="libwxgtk2.8-dev"/>
</package>
Modified: pkg/trunk/visualization/wx_ros/wx_camera_panel/manifest.xml
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_camera_panel/manifest.xml 2008-07-31 21:18:15 UTC (rev 2391)
+++ pkg/trunk/visualization/wx_ros/wx_camera_panel/manifest.xml 2008-07-31 21:33:53 UTC (rev 2392)
@@ -14,5 +14,7 @@
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lwxcamerapanels"/>
</export>
+<sysdepend os="Ubuntu" version="7.04" package="libwxgtk2.8-dev"/>
+<sysdepend os="Ubuntu" version="8.04.1" package="libwxgtk2.8-dev"/>
</package>
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml 2008-07-31 21:18:15 UTC (rev 2391)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml 2008-07-31 21:33:53 UTC (rev 2392)
@@ -10,5 +10,7 @@
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lwxtopicdisplay"/>
</export>
+<sysdepend os="Ubuntu" version="7.04" package="libwxgtk2.8-dev"/>
+<sysdepend os="Ubuntu" version="8.04.1" package="libwxgtk2.8-dev"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-07-31 21:34:24
|
Revision: 2393
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2393&view=rev
Author: gerkey
Date: 2008-07-31 21:34:32 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
fixed minor build problems caught by buildbot
Modified Paths:
--------------
pkg/trunk/3rdparty/opencv_latest/Makefile
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
Modified: pkg/trunk/3rdparty/opencv_latest/Makefile
===================================================================
--- pkg/trunk/3rdparty/opencv_latest/Makefile 2008-07-31 21:33:53 UTC (rev 2392)
+++ pkg/trunk/3rdparty/opencv_latest/Makefile 2008-07-31 21:34:32 UTC (rev 2393)
@@ -6,7 +6,7 @@
.PHONY: $(CVS_DIR)
$(CVS_DIR):
- cvs -d:pserver:ano...@op...:/cvsroot/opencvlibrary login
+ cvs -d:pserver:anonymous:@opencvlibrary.cvs.sourceforge.net:/cvsroot/opencvlibrary login
if [ ! -d $(CVS_DIR) ]; then \
cvs -z3 -d:pserver:ano...@op...:/cvsroot/opencvlibrary co -P -d $(CVS_DIR) opencv; \
else \
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-07-31 21:33:53 UTC (rev 2392)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-07-31 21:34:32 UTC (rev 2393)
@@ -288,7 +288,7 @@
en.odom.vel.th = pdata->vel.pa;
en.odom.stall = pdata->stall;
- en.odom.header.frame_id = tf.lookup("FRAMEID_ODOM");
+ en.odom.header.frame_id = en.tf.lookup("FRAMEID_ODOM");
en.odom.header.stamp.sec = (long long unsigned int)floor(hdr->timestamp);
en.odom.header.stamp.sec = (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-31 21:35:07
|
Revision: 2394
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2394&view=rev
Author: isucan
Date: 2008-07-31 21:35:13 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
a bit of code reorganization... need more testing
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/include/
pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h
Added: pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h 2008-07-31 21:35:13 UTC (rev 2394)
@@ -0,0 +1,119 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include <planning_models/kinematic.h>
+
+/** This class configures an instance of SpaceInformationKinematic with data from a KinematicModel */
+class SpaceInformationXMLModel : public ompl::SpaceInformationKinematic
+{
+ public:
+ SpaceInformationXMLModel(planning_models::KinematicModel *kmodel, int groupID = -1, double divisions = 20.0) : SpaceInformationKinematic()
+ {
+ m_kmodel = kmodel;
+ m_groupID = groupID;
+ m_divisions = divisions;
+
+ m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
+ m_stateComponent.resize(m_stateDimension);
+
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ {
+ if (m_stateComponent[i].type == StateComponent::UNKNOWN)
+ m_stateComponent[i].type = StateComponent::NORMAL;
+ int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
+ m_stateComponent[i].minValue = m_kmodel->stateBounds[p ];
+ m_stateComponent[i].maxValue = m_kmodel->stateBounds[p + 1];
+ m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
+
+ for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
+ if (m_kmodel->floatingJoints[j] == p)
+ {
+ m_floatingJoints.push_back(i);
+ m_stateComponent[i + 3].type = StateComponent::QUATERNION;
+ break;
+ }
+
+ for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
+ if (m_kmodel->planarJoints[j] == p)
+ {
+ m_planarJoints.push_back(i);
+ break;
+ }
+ }
+ }
+
+ virtual ~SpaceInformationXMLModel(void)
+ {
+ }
+
+ void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
+ {
+ for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
+ {
+ int id = m_floatingJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ m_stateComponent[id + 2].minValue = z0;
+ m_stateComponent[id + 2].maxValue = z1;
+ for (int j = 0 ; j < 3 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ }
+
+ void setPlanningArea(double x0, double y0, double x1, double y1)
+ {
+ for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
+ {
+ int id = m_planarJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ for (int j = 0 ; j < 2 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ }
+
+ protected:
+
+ double m_divisions;
+ planning_models::KinematicModel *m_kmodel;
+ int m_groupID;
+ std::vector<int> m_floatingJoints;
+ std::vector<int> m_planarJoints;
+
+};
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-31 21:35:13 UTC (rev 2394)
@@ -88,8 +88,10 @@
#include <urdf/URDF.h>
#include <collision_space/environmentODE.h>
-#include <ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h>
+#include <ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h>
+#include "SpaceInformationXMLModel.h"
+
#include <vector>
#include <string>
#include <sstream>
@@ -120,7 +122,7 @@
m_collisionSpace->unlock();
// temp obstacle
- double sphere[6] = {0.8,0.2,0.4};
+ double sphere[6] = {2.8,0.2,0.4};
m_collisionSpace->addPointCloud(1, sphere, 0.15);
#ifdef DISPLAY_ODE_SPACES
@@ -215,10 +217,13 @@
return false;
/* set the workspace volume for planning */
- static_cast<SpaceInformationNode*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
- req.volumeMax.x, req.volumeMax.y, req.volumeMax.z);
+ // only area or volume should go through... not sure what happens on really complicated models where
+ // we have both multiple planar and multiple floating joints...
+ static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningArea(req.volumeMin.x, req.volumeMin.y,
+ req.volumeMax.x, req.volumeMax.y);
+ static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
+ req.volumeMax.x, req.volumeMax.y, req.volumeMax.z);
-
/* set the starting state */
ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
@@ -385,56 +390,7 @@
planning_models::KinematicModel *kmodel;
int groupID;
};
-
- class SpaceInformationNode : public ompl::SpaceInformationKinematic
- {
- public:
- SpaceInformationNode(Model *model) : SpaceInformationKinematic()
- {
- m_m = model;
- m_divisions = 20.0;
-
- m_stateDimension = m_m->groupID >= 0 ? m_m->kmodel->groupStateIndexList[m_m->groupID].size() : m_m->kmodel->stateDimension;
- m_stateComponent.resize(m_stateDimension);
-
- for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
- {
- m_stateComponent[i].type = StateComponent::NORMAL;
- unsigned int p = m_m->groupID >= 0 ? m_m->kmodel->groupStateIndexList[m_m->groupID][i] * 2 : i * 2;
- m_stateComponent[i].minValue = m_m->kmodel->stateBounds[p ];
- m_stateComponent[i].maxValue = m_m->kmodel->stateBounds[p + 1];
- m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
- }
- }
-
- virtual ~SpaceInformationNode(void)
- {
- }
-
- void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
- {
- if (m_m->groupID < 0)
- for (unsigned int i = 0 ; i < m_m->kmodel->floatingJoints.size() ; ++i)
- {
- int id = m_m->kmodel->floatingJoints[i];
- m_stateComponent[id ].minValue = x0;
- m_stateComponent[id ].maxValue = x1;
- m_stateComponent[id + 1].minValue = y0;
- m_stateComponent[id + 1].maxValue = y1;
- m_stateComponent[id + 2].minValue = z0;
- m_stateComponent[id + 2].maxValue = z1;
- for (int j = 0 ; j < 3 ; ++j)
- m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
- }
- }
-
- protected:
-
- double m_divisions;
- Model *m_m;
-
- };
-
+
std_msgs::PointCloudFloat32 m_cloud;
collision_space::EnvironmentModel *m_collisionSpace;
std::map<std::string, Model*> m_models;
@@ -452,9 +408,9 @@
void createMotionPlanningInstances(Model* model)
{
Planner p;
- p.si = new SpaceInformationNode(model);
+ p.si = new SpaceInformationXMLModel(model->kmodel, model->groupID);
p.si->setStateValidFn(isStateValid, reinterpret_cast<void*>(model));
- p.mp = new ompl::LazyRRT(p.si);
+ p.mp = new ompl::RRT(p.si);
p.type = 0;
model->planners.push_back(p);
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-07-31 21:35:13 UTC (rev 2394)
@@ -161,9 +161,14 @@
struct StateComponent
{
+ StateComponent(void)
+ {
+ type = UNKNOWN;
+ }
+
enum
- { NORMAL, FIXED }
- type;
+ { UNKNOWN, NORMAL, QUATERNION, FIXED }
+ type;
double minValue;
double maxValue;
double resolution;
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-07-31 21:35:13 UTC (rev 2394)
@@ -48,7 +48,7 @@
robot_srvs::KinematicMotionPlan::request req;
robot_srvs::KinematicMotionPlan::response res;
- req.model_id = "pr2::base+rightArm";
+ req.model_id = "pr2::rightArm";
req.start_state.set_vals_size(32);
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
@@ -57,14 +57,14 @@
for (unsigned int i = 18 ; i < 25 ; ++i)
req.start_state.vals[i] = 0.1;
- req.goal_state.set_vals_size(11);
+ req.goal_state.set_vals_size(7);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = -0.4;
req.allowed_time = 15.0;
req.volumeMin.x = -1.0; req.volumeMin.y = -1.0; req.volumeMin.z = -1.0;
- req.volumeMax.x = -1.0; req.volumeMax.y = -1.0; req.volumeMax.z = -1.0;
+ req.volumeMax.x = 1.0; req.volumeMax.y = 1.0; req.volumeMax.z = 1.0;
if (ros::service::call("plan_kinematic_path", req, res))
{
Modified: pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-31 21:35:13 UTC (rev 2394)
@@ -107,7 +107,7 @@
/* relevant joint information */
enum
{
- UNKNOWN, FIXED, REVOLUTE, PRISMATIC, FLOATING
+ UNKNOWN, FIXED, REVOLUTE, PRISMATIC, PLANAR, FLOATING
} type;
double axis[3];
double anchor[3];
@@ -141,6 +141,9 @@
for (unsigned int i = 0 ; i < after.size() ; ++i)
delete after[i];
}
+
+ /** Name of the link */
+ std::string name;
/** Joint that connects this link to the parent link */
Joint *before;
@@ -209,6 +212,12 @@
planning, so the indices are provided here for
convenience. */
std::vector<int> floatingJoints;
+
+ /** The list of index values where planar joints
+ start. These joints need special attention in motion
+ planning, so the indices are provided here for
+ convenience. */
+ std::vector<int> planarJoints;
/* For each group, we have a list of index values in the
* computed state information that describe the components of
@@ -254,6 +263,9 @@
/** Cumulative list of floating joints */
std::vector<int> floatingJoints;
+
+ /** Cumulative list of planar joints */
+ std::vector<int> planarJoints;
/** Cumulative state dimension */
unsigned int stateDimension;
@@ -277,8 +289,13 @@
private:
+ /** Build the needed datastructure for a joint */
void buildChainJ(Robot *robot, Link *parent, Joint *joint, robot_desc::URDF::Link *urdfLink, robot_desc::URDF &model);
+
+ /** Build the needed datastructure for a link */
void buildChainL(Robot *robot, Joint *parent, Link *link, robot_desc::URDF::Link *urdfLink, robot_desc::URDF &model);
+
+ /** Construct the list of groups the model knows about (the ones marked with the 'plan' attribute) */
void constructGroupList(robot_desc::URDF &model);
};
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-31 21:35:13 UTC (rev 2394)
@@ -34,6 +34,7 @@
#include <planning_models/kinematic.h>
#include <cstdio>
+#include <cmath>
void planning_models::KinematicModel::Robot::computeTransforms(const double *params, int groupID)
{
@@ -81,8 +82,13 @@
varTrans.setPosition(dx, dy, dz);
}
break;
+ case Joint::PLANAR:
+ varTrans.setPosition(params[0], params[1], 0.0);
+ varTrans.setAxisAngle(0.0, 0.0, 1.0, params[2]);
+ break;
case Joint::FLOATING:
varTrans.setPosition(params[0], params[1], params[2]);
+ varTrans.setQuaternion(params[3], params[4], params[5], params[6]);
break;
default:
break;
@@ -182,6 +188,10 @@
/* copy floating joints*/
for (unsigned int j = 0 ; j < m_robots[i]->floatingJoints.size() ; ++j)
floatingJoints.push_back(stateDimension + m_robots[i]->floatingJoints[j]);
+
+ /* copy planar joints*/
+ for (unsigned int j = 0 ; j < m_robots[i]->planarJoints.size() ; ++j)
+ planarJoints.push_back(stateDimension + m_robots[i]->planarJoints[j]);
/* copy group roots */
for (unsigned int j = 0 ; j < m_robots[i]->groupChainStart.size() ; ++j)
@@ -235,10 +245,18 @@
{
case robot_desc::URDF::Link::Joint::FLOATING:
joint->type = Joint::FLOATING;
- joint->usedParams = 3;
- robot->stateBounds.insert(robot->stateBounds.end(), 6, 0.0);
+ joint->usedParams = 7;
+ robot->stateBounds.insert(robot->stateBounds.end(), 14, 0.0);
robot->floatingJoints.push_back(robot->stateDimension);
break;
+ case robot_desc::URDF::Link::Joint::PLANAR:
+ joint->type = Joint::PLANAR;
+ joint->usedParams = 3;
+ robot->stateBounds.insert(robot->stateBounds.end(), 4, 0.0);
+ robot->stateBounds.push_back(-M_PI);
+ robot->stateBounds.push_back(M_PI);
+ robot->planarJoints.push_back(robot->stateDimension);
+ break;
case robot_desc::URDF::Link::Joint::FIXED:
joint->type = Joint::FIXED;
joint->usedParams = 0;
@@ -296,6 +314,7 @@
void planning_models::KinematicModel::buildChainL(Robot *robot, Joint *parent, Link* link, robot_desc::URDF::Link* urdfLink, robot_desc::URDF &model)
{
+ link->name = urdfLink->name;
link->before = parent;
robot->links.push_back(link);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-31 21:47:09
|
Revision: 2396
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2396&view=rev
Author: isucan
Date: 2008-07-31 21:47:16 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
more information reporting in kinematic model building
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-31 21:45:54 UTC (rev 2395)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-31 21:47:16 UTC (rev 2396)
@@ -252,6 +252,11 @@
goal->state->values[i] = req.goal_state.vals[i];
goal->threshold = 1e-6;
p.si->setGoal(goal);
+
+ printf("=======================================\n");
+ m->kmodel->printModelInfo();
+ p.si->printSettings();
+ printf("=======================================\n");
/* do the planning */
m_collisionSpace->lock();
Modified: pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-31 21:45:54 UTC (rev 2395)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-31 21:47:16 UTC (rev 2396)
@@ -39,6 +39,7 @@
#include <libTF/Pose3D.h>
#include <vector>
#include <string>
+#include <cstdio>
/** @htmlinclude ../../manifest.html
@@ -257,7 +258,8 @@
int getGroupID(const std::string &group) const;
void computeTransforms(const double *params, int groupID = -1);
-
+ void printModelInfo(FILE *out = stdout) const;
+
/** A transform that is applied to the entire model */
libTF::Pose3D rootTransform;
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-31 21:45:54 UTC (rev 2395)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-31 21:47:16 UTC (rev 2396)
@@ -33,7 +33,6 @@
*********************************************************************/
#include <planning_models/kinematic.h>
-#include <cstdio>
#include <cmath>
void planning_models::KinematicModel::Robot::computeTransforms(const double *params, int groupID)
@@ -370,3 +369,41 @@
if (link->after.size() == 0)
robot->leafs.push_back(link);
}
+
+void planning_models::KinematicModel::printModelInfo(FILE *out) const
+{
+ fprintf(out, "Number of robots = %d\n", getRobotCount());
+ fprintf(out, "Complete model state dimension = %d\n", stateDimension);
+
+ fprintf(out, "State bounds: ");
+ for (unsigned int i = 0 ; i < stateDimension ; ++i)
+ fprintf(out, "[%f, %f] ", stateBounds[2 * i], stateBounds[2 * i + 1]);
+ fprintf(out, "\n");
+
+ fprintf(out, "Floating joints at: ");
+ for (unsigned int i = 0 ; i < floatingJoints.size() ; ++i)
+ fprintf(out, "%d ", floatingJoints[i]);
+ fprintf(out, "\n");
+
+ fprintf(out, "Planar joints at: ");
+ for (unsigned int i = 0 ; i < planarJoints.size() ; ++i)
+ fprintf(out, "%d ", planarJoints[i]);
+ fprintf(out, "\n");
+
+ fprintf(out, "Available groups: ");
+ std::vector<std::string> l;
+ getGroups(l);
+ for (unsigned int i = 0 ; i < l.size() ; ++i)
+ fprintf(out, "%s ", l[i].c_str());
+ fprintf(out, "\n");
+
+ for (unsigned int i = 0 ; i < l.size() ; ++i)
+ {
+ int gid = getGroupID(l[i]);
+ fprintf(out, "Group %s has %d roots\n", l[i].c_str(), groupChainStart[gid].size());
+ fprintf(out, "The state components for this group are: ");
+ for (unsigned int j = 0 ; j < groupStateIndexList[gid].size() ; ++j)
+ fprintf(out, "%d ", groupStateIndexList[gid][j]);
+ fprintf(out, "\n");
+ }
+}
Modified: pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-31 21:45:54 UTC (rev 2395)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-31 21:47:16 UTC (rev 2396)
@@ -56,39 +56,6 @@
spaces.displaySpaces();
}
-void printModelInfo(planning_models::KinematicModel *m)
-{
- printf("Number of robots = %d\n", m->getRobotCount());
- printf("Complete model state dimension = %d\n", m->stateDimension);
-
- printf("State bounds: ");
- for (unsigned int i = 0 ; i < m->stateDimension ; ++i)
- printf("[%f, %f] ", m->stateBounds[2 * i], m->stateBounds[2 * i + 1]);
- printf("\n");
-
- printf("Floating joints at: ");
- for (unsigned int i = 0 ; i < m->floatingJoints.size() ; ++i)
- printf("%d ", m->floatingJoints[i]);
- printf("\n");
-
- printf("Available groups: ");
- std::vector<std::string> l;
- m->getGroups(l);
- for (unsigned int i = 0 ; i < l.size() ; ++i)
- printf("%s ", l[i].c_str());
- printf("\n");
-
- for (unsigned int i = 0 ; i < l.size() ; ++i)
- {
- int gid = m->getGroupID(l[i]);
- printf("Group %s has %d roots\n", l[i].c_str(), m->groupChainStart[gid].size());
- printf("The state components for this group are: ");
- for (unsigned int j = 0 ; j < m->groupStateIndexList[gid].size() ; ++j)
- printf("%d ", m->groupStateIndexList[gid][j]);
- printf("\n");
- }
-}
-
int main(int argc, char **argv)
{
// TODO: add usage message
@@ -103,7 +70,7 @@
planning_models::KinematicModel *m = new planning_models::KinematicModel();
m->build(model);
km->addRobotModel(m);
- printModelInfo(m);
+ m->printModelInfo();
double *param = new double[m->stateDimension];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-01 00:19:48
|
Revision: 2414
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2414&view=rev
Author: isucan
Date: 2008-08-01 00:19:57 +0000 (Fri, 01 Aug 2008)
Log Message:
-----------
using the new parser data in the kinematic model
Modified Paths:
--------------
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/URDF.cpp
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/URDF.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/URDF.cpp 2008-08-01 00:13:53 UTC (rev 2413)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/URDF.cpp 2008-08-01 00:19:57 UTC (rev 2414)
@@ -1551,7 +1551,7 @@
{
if (joint->axis[0] == 0.0 && joint->axis[1] == 0.0 && joint->axis[2] == 0.0)
fprintf(stderr, "Joint '%s' in link '%s' does not seem to have its axis properly set\n", joint->name.c_str(), links[i]->name.c_str());
- if (joint->isSet["limit"] && joint->limit[0] == 0.0 && joint->limit[1] == 0.0)
+ if ((joint->isSet["limit"] || joint->type == Link::Joint::PRISMATIC) && joint->limit[0] == 0.0 && joint->limit[1] == 0.0)
fprintf(stderr, "Joint '%s' in link '%s' does not seem to have its limits properly set\n", joint->name.c_str(), links[i]->name.c_str());
}
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-08-01 00:13:53 UTC (rev 2413)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-08-01 00:19:57 UTC (rev 2414)
@@ -278,6 +278,12 @@
break;
}
+ if (!urdfLink->joint->isSet["limit"] && joint->type == Joint::REVOLUTE)
+ {
+ joint->limit[0] = -M_PI;
+ joint->limit[1] = M_PI;
+ }
+
/** construct the inGroup bitvector */
std::vector<std::string> gnames;
model.getGroupNames(gnames);
Modified: pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-08-01 00:13:53 UTC (rev 2413)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-08-01 00:19:57 UTC (rev 2414)
@@ -79,12 +79,12 @@
m->computeTransforms(param);
km->updateRobotModel(0);
-
+ /*
for (unsigned int i = 0 ; i < m->stateDimension ; ++i)
param[i] = +0.3;
m->computeTransforms(param, m->getGroupID("pr2::base+rightArm"));
km->updateRobotModel(0);
-
+ */
EnvironmentModelODE* okm = dynamic_cast<EnvironmentModelODE*>(km);
spaces.addSpace(okm->getODESpace(), 1.0f, 0.3f, 0.0f);
for (unsigned int i = 0 ; i < okm->getModelCount() ; ++i)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-01 01:15:59
|
Revision: 2426
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2426&view=rev
Author: isucan
Date: 2008-08-01 01:16:08 +0000 (Fri, 01 Aug 2008)
Log Message:
-----------
fixed state space bounding box problem
Modified Paths:
--------------
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-08-01 01:08:27 UTC (rev 2425)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-08-01 01:16:08 UTC (rev 2426)
@@ -240,6 +240,12 @@
joint->anchor[0] = urdfLink->joint->anchor[0];
joint->anchor[1] = urdfLink->joint->anchor[1];
joint->anchor[2] = urdfLink->joint->anchor[2];
+ if (!urdfLink->joint->isSet["limit"] && urdfLink->joint->type == robot_desc::URDF::Link::Joint::REVOLUTE)
+ {
+ joint->limit[0] = -M_PI;
+ joint->limit[1] = M_PI;
+ }
+
switch (urdfLink->joint->type)
{
case robot_desc::URDF::Link::Joint::FLOATING:
@@ -278,12 +284,6 @@
break;
}
- if (!urdfLink->joint->isSet["limit"] && joint->type == Joint::REVOLUTE)
- {
- joint->limit[0] = -M_PI;
- joint->limit[1] = M_PI;
- }
-
/** construct the inGroup bitvector */
std::vector<std::string> gnames;
model.getGroupNames(gnames);
Modified: pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-08-01 01:08:27 UTC (rev 2425)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-08-01 01:16:08 UTC (rev 2426)
@@ -68,6 +68,7 @@
EnvironmentModel *km = new EnvironmentModelODE();
planning_models::KinematicModel *m = new planning_models::KinematicModel();
+ m->setVerbose(true);
m->build(model);
km->addRobotModel(m);
m->printModelInfo();
@@ -97,7 +98,6 @@
printf("Collision: %d\n", km->isCollision(0));
-
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-01 16:42:38
|
Revision: 2442
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2442&view=rev
Author: isucan
Date: 2008-08-01 16:42:47 +0000 (Fri, 01 Aug 2008)
Log Message:
-----------
removed anchor definition
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-01 16:37:32 UTC (rev 2441)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-01 16:42:47 UTC (rev 2442)
@@ -285,6 +285,15 @@
/* copy the solution to the result */
if (ok)
{
+
+#ifdef DISPLAY_ODE_SPACES
+ if (m->groupID >= 0)
+ {
+ /* set the pose of the whole robot */
+ m->kmodel->computeTransforms(req.start_state.vals);
+ m->collisionSpace->updateRobotModel(m->collisionSpaceID);
+ }
+#endif
ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
res.path.set_states_size(path->states.size());
for (unsigned int i = 0 ; i < path->states.size() ; ++i)
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-01 16:37:32 UTC (rev 2441)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-01 16:42:47 UTC (rev 2442)
@@ -48,18 +48,17 @@
robot_srvs::KinematicMotionPlan::request req;
robot_srvs::KinematicMotionPlan::response res;
- req.model_id = "pr2::rightArm";
+ req.model_id = "pr2::base";
req.start_state.set_vals_size(32);
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
req.start_state.vals[i] = 0.0;
- for (unsigned int i = 18 ; i < 25 ; ++i)
- req.start_state.vals[i] = 0.1;
-
- req.goal_state.set_vals_size(7);
+ req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
- req.goal_state.vals[i] = -0.4;
+ req.goal_state.vals[i] = 0.0;
+ req.goal_state.vals[0] = 1.0;
+ req.goal_state.vals[2] = M_PI/2;
req.allowed_time = 15.0;
@@ -70,6 +69,12 @@
{
unsigned int nstates = res.path.get_states_size();
printf("Obtained solution path with %u states\n", nstates);
+ for (unsigned int i = 0 ; i < nstates ; ++i)
+ {
+ for (unsigned int j = 0 ; j < res.path.states[i].get_vals_size() ; ++j)
+ printf("%f ", res.path.states[i].vals[j]);
+ printf("\n");
+ }
}
else
fprintf(stderr, "Service 'plan_kinematic_path' failed\n");
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-01 16:37:32 UTC (rev 2441)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-01 16:42:47 UTC (rev 2442)
@@ -428,7 +428,6 @@
<rpy> 0 0 0</rpy> <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
<xyz> 0.25 0.1 0.3+wheel_clearance_offset</xyz> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
- <xyz> 0 0 0</xyz> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
<joint type="planar">
</joint>
@@ -997,6 +996,10 @@
<!-- Define groups of links; a link may be part of multiple groups -->
+ <group name="base" flags="plan">
+ base
+ </group>
+
<group name="leftArm" flags="plan">
shoulder_pan_left
shoulder_pitch_left
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-08-01 17:08:35
|
Revision: 2443
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2443&view=rev
Author: gerkey
Date: 2008-08-01 17:08:35 +0000 (Fri, 01 Aug 2008)
Log Message:
-----------
Added system (non-ROS) dependencies and SVN urls to manifests.
Modified Paths:
--------------
pkg/trunk/3rdparty/ffmpeg/manifest.xml
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/3rdparty/gmapping/manifest.xml
pkg/trunk/3rdparty/libsunflower/manifest.xml
pkg/trunk/3rdparty/opende/manifest.xml
pkg/trunk/3rdparty/player/manifest.xml
pkg/trunk/3rdparty/stage/manifest.xml
pkg/trunk/3rdparty/trex/manifest.xml
pkg/trunk/3rdparty/xenomai/manifest.xml
pkg/trunk/drivers/cam/axis_cam/manifest.xml
pkg/trunk/drivers/cam/ptgrey/flea2/manifest.xml
pkg/trunk/visualization/ogre_tools/manifest.xml
pkg/trunk/visualization/pr2_gui/manifest.xml
pkg/trunk/visualization/wx_ros/wx_camera_panel/manifest.xml
pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml
Modified: pkg/trunk/3rdparty/ffmpeg/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/ffmpeg/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/ffmpeg/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -13,5 +13,6 @@
<cpp cflags="-I${prefix}/ffmpeg/include" lflags="-L${prefix}/ffmpeg/lib -Wl,-rpath,${prefix}/ffmpeg/lib -lavcodec -lavdevice -lavformat -lavutil" />
<doxymaker external="http://ffmpeg.mplayerhq.hu/documentation.html"/>
</export>
+<versioncontrol type="svn" url="svn://svn.mplayerhq.hu/ffmpeg/trunk"/>
</package>
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -14,6 +14,7 @@
<cpp lflags="-Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="-I${prefix}/gazebo/include -I${prefix}/gazebo-svn/server/physics -I${prefix}/gazebo-svn/server/sensors -I${prefix}/gazebo-svn/server/sensors/ray -I${prefix}/gazebo-svn/server/sensors/camera -I${prefix}/gazebo-svn/server -I${prefix}/gazebo-svn/server/rendering"/>
<doxymaker external="http://playerstage.sourceforge.net/doc/Gazebo-manual-svn-html/"/>
</export>
+<versioncontrol type="svn" url="https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/gazebo/trunk"/>
</package>
Modified: pkg/trunk/3rdparty/gmapping/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gmapping/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/gmapping/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -14,10 +14,12 @@
<doxymaker external="http://openslam.org"/>
</export>
-<sysdepend os="Ubuntu" version="7.04" package="qt3-dev-tools"/>
-<sysdepend os="Ubuntu" version="7.04" package="libqt3-headers"/>
-<sysdepend os="Ubuntu" version="8.04.1" package="qt3-dev-tools"/>
-<sysdepend os="Ubuntu" version="8.04.1" package="libqt3-headers"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="qt3-dev-tools"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libqt3-headers"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="qt3-dev-tools"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libqt3-headers"/>
+
+<versioncontrol type="svn" url="https://svn.openslam.org/data/svn/gmapping/trunk"/>
</package>
Modified: pkg/trunk/3rdparty/libsunflower/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/libsunflower/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/libsunflower/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -15,4 +15,5 @@
<doxymaker external="http://libsunflower.sourceforge.net/"/>
</export>
<depend package="estar"/>
+<versioncontrol type="svn" url="https://libsunflower.svn.sourceforge.net/svnroot/libsunflower/trunk/sunflower"/>
</package>
Modified: pkg/trunk/3rdparty/opende/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/opende/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/opende/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -12,4 +12,5 @@
<cpp lflags="-Wl,-rpath,${prefix}/opende/lib `${prefix}/opende/bin/ode-config --libs`" cflags="`${prefix}/opende/bin/ode-config --cflags`"/>
<doxymaker external="http://opende.sf.net"/>
</export>
+<versioncontrol type="svn" url="https://opende.svn.sourceforge.net/svnroot/opende/trunk"/>
</package>
Modified: pkg/trunk/3rdparty/player/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/player/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/player/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -16,6 +16,7 @@
<cpp lflags="-Wl,-rpath,${prefix}/player/lib -L${prefix}/player/lib -lplayercore -lplayerxdr -lplayerutils -lplayererror -lplayerdrivers" cflags="-I${prefix}/player/include/player-2.2"/>
<doxymaker external="http://playerstage.sourceforge.net/doc/Player-cvs/player"/>
</export>
+<versioncontrol type="svn" url="https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/player/trunk"/>
</package>
Modified: pkg/trunk/3rdparty/stage/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/stage/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/stage/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -15,6 +15,7 @@
<cpp lflags="-Wl,-rpath,${prefix}/stage/lib `PKG_CONFIG_PATH=${prefix}/stage/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --libs stage`" cflags="`PKG_CONFIG_PATH=${prefix}/stage/lib/pkgconfig:$PKG_CONFIG_PATH pkg-config --cflags stage`"/>
<doxymaker external="http://playerstage.sourceforge.net/doc/stage-cvs"/>
</export>
+<versioncontrol type="svn" url="https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/stage/branches/stage-ros"/>
</package>
Modified: pkg/trunk/3rdparty/trex/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/trex/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/trex/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -15,6 +15,9 @@
<cpp lflags="-Xlinker -rpath ${prefix}/PLASMA/build/lib -L${prefix}/PLASMA/build/lib" cflags="-I${prefix}/TREX/agent/base"/>
<doxymaker external="https://babelfish.arc.nasa.gov/trac/europa/wiki"/>
</export>
+<versioncontrol type="svn" url="https://babelfish.arc.nasa.gov/svn/europa/PLASMA/trunk"/>
+<versioncontrol type="svn" url="https://babelfish.arc.nasa.gov/svn/europa/PlanWorks/trunk"/>
+<versioncontrol type="svn" url="https://prdev.willowgarage.com/svn/scratch/TREX"/>
</package>
Modified: pkg/trunk/3rdparty/xenomai/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/xenomai/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/3rdparty/xenomai/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -14,6 +14,7 @@
<cpp lflags="`${prefix}/xenomai/bin/xeno-config --posix-ldflags`" cflags="`${prefix}/xenomai/bin/xeno-config --posix-cflags`"/>
<doxymaker external="http://www.xenomai.org/documentation/trunk/html/api/index.html"/>
</export>
+<versioncontrol type="svn" url="http://svn.gna.org/svn/xenomai/trunk"/>
</package>
Modified: pkg/trunk/drivers/cam/axis_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -17,7 +17,7 @@
<depend package="rosthread"/>
<depend package="string_utils"/>
<depend package="image_utils"/>
-<sysdepend package="libcurl3-openssl-dev" os="ubuntu" version="7.10-gutsy"/>
+<sysdepend package="libcurl3-openssl-dev" os="ubuntu" version="7.04-feisty"/>
<sysdepend package="libcurl4-openssl-dev" os="ubuntu" version="8.04-hardy"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -laxis_cam -lcurl"/>
Modified: pkg/trunk/drivers/cam/ptgrey/flea2/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/ptgrey/flea2/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/drivers/cam/ptgrey/flea2/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -8,6 +8,6 @@
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lflea2 -ldc1394_control -lraw1394"/>
</export>
- <sysdepend os="Ubuntu" version="7.04" package="libdc1394-13-dev"/>
- <sysdepend os="Ubuntu" version="8.04.1" package="libdc1394-13-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libdc1394-13-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libdc1394-13-dev"/>
</package>
Modified: pkg/trunk/visualization/ogre_tools/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_tools/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/visualization/ogre_tools/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -12,7 +12,7 @@
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -logretools"/>
</export>
- <sysdepend os="Ubuntu" version="7.04" package="libwxgtk2.8-dev"/>
- <sysdepend os="Ubuntu" version="8.04.1" package="libwxgtk2.8-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="libwxgtk2.8-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libwxgtk2.8-dev"/>
</package>
Modified: pkg/trunk/visualization/pr2_gui/manifest.xml
===================================================================
--- pkg/trunk/visualization/pr2_gui/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/visualization/pr2_gui/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -18,7 +18,7 @@
<export>
</export>
-<sysdepend os="Ubuntu" version="7.04" package="libwxgtk2.8-dev"/>
-<sysdepend os="Ubuntu" version="8.04.1" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libwxgtk2.8-dev"/>
</package>
Modified: pkg/trunk/visualization/wx_ros/wx_camera_panel/manifest.xml
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_camera_panel/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/visualization/wx_ros/wx_camera_panel/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -14,7 +14,7 @@
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lwxcamerapanels"/>
</export>
-<sysdepend os="Ubuntu" version="7.04" package="libwxgtk2.8-dev"/>
-<sysdepend os="Ubuntu" version="8.04.1" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libwxgtk2.8-dev"/>
</package>
Modified: pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml 2008-08-01 16:42:47 UTC (rev 2442)
+++ pkg/trunk/visualization/wx_ros/wx_topic_display/manifest.xml 2008-08-01 17:08:35 UTC (rev 2443)
@@ -10,7 +10,7 @@
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lwxtopicdisplay"/>
</export>
-<sysdepend os="Ubuntu" version="7.04" package="libwxgtk2.8-dev"/>
-<sysdepend os="Ubuntu" version="8.04.1" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libwxgtk2.8-dev"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-01 21:17:10
|
Revision: 2463
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2463&view=rev
Author: isucan
Date: 2008-08-01 21:17:18 +0000 (Fri, 01 Aug 2008)
Log Message:
-----------
better code for kinematic model
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-01 21:17:02 UTC (rev 2462)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-01 21:17:18 UTC (rev 2463)
@@ -215,10 +215,19 @@
Model *m = m_models[req.model_id];
Planner &p = m->planners[0];
- const int dim = req.goal_state.vals_size;
+ if (m->kmodel->stateDimension != req.start_state.get_vals_size())
+ {
+ fprintf(stderr, "Dimension of start state expected to be %u but was received as %u\n", m->kmodel->stateDimension, req.start_state.get_vals_size());
+ return false;
+ }
+
+ const int dim = req.goal_state.get_vals_size();
if ((int)p.si->getStateDimension() != dim)
+ {
+ fprintf(stderr, "Dimension of goal state expected to be %u but was received as %d\n", p.si->getStateDimension(), dim);
return false;
-
+ }
+
/* set the workspace volume for planning */
// only area or volume should go through... not sure what happens on really complicated models where
// we have both multiple planar and multiple floating joints...
@@ -310,6 +319,8 @@
}
}
+ else
+ res.path.set_states_size(0);
/* cleanup */
p.si->clearGoal();
@@ -344,30 +355,36 @@
printf("\n\nCreating new kinematic model:\n");
/* create a model for the whole robot (with the name given in the file) */
- Model *model = new Model();
planning_models::KinematicModel *kmodel = new planning_models::KinematicModel();
kmodel->setVerbose(true);
kmodel->build(*file);
+
+ /* add the model to the collision space */
unsigned int cid = m_collisionSpace->addRobotModel(kmodel);
+
+ /* set the data for the model */
+ Model *model = new Model();
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
- model->kmodel = m_collisionSpace->getModel(cid);
- m_models[file->getRobotName()] = model;
+ model->kmodel = kmodel;
createMotionPlanningInstances(model);
+ /* remember the model by the robot's name */
+ m_models[file->getRobotName()] = model;
+
+ /* create a model for each group */
std::vector<std::string> groups;
kmodel->getGroups(groups);
- /* create a model for each group */
for (unsigned int i = 0 ; i < groups.size() ; ++i)
{
Model *model = new Model();
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
- model->kmodel = m_collisionSpace->getModel(cid);
- model->groupID = model->kmodel->getGroupID(groups[i]);
+ model->kmodel = kmodel;
+ model->groupID = kmodel->getGroupID(groups[i]);
+ createMotionPlanningInstances(model);
m_models[groups[i]] = model;
- createMotionPlanningInstances(model);
}
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-01 21:17:02 UTC (rev 2462)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-01 21:17:18 UTC (rev 2463)
@@ -50,17 +50,19 @@
req.model_id = "pr2::base";
- req.start_state.set_vals_size(32);
+ req.start_state.set_vals_size(34);
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
req.start_state.vals[i] = 0.0;
-
+ req.start_state.vals[0] = -0.5;
+
req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = 0.0;
- req.goal_state.vals[0] = 1.0;
+ req.goal_state.vals[0] = 0.75;
+ req.goal_state.vals[1] = 0.75;
req.goal_state.vals[2] = M_PI/2;
- req.allowed_time = 15.0;
+ req.allowed_time = 10.0;
req.volumeMin.x = -1.0; req.volumeMin.y = -1.0; req.volumeMin.z = -1.0;
req.volumeMax.x = 1.0; req.volumeMax.y = 1.0; req.volumeMax.z = 1.0;
Modified: pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-08-01 21:17:02 UTC (rev 2462)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-08-01 21:17:18 UTC (rev 2463)
@@ -84,6 +84,7 @@
axis[0] = axis[1] = axis[2] = 0.0;
anchor[0] = anchor[1] = anchor[2] = 0.0;
limit[0] = limit[1] = 0.0;
+ robotRoot = false;
type = UNKNOWN;
}
@@ -97,6 +98,9 @@
Link *before;
Link *after;
+ /** Flag for marking joints that are roots for the entire robot */
+ bool robotRoot;
+
/** The range of indices in the parameter vector that
needed to access information about the position of this
joint */
@@ -231,9 +235,6 @@
/** The model that owns this robot */
KinematicModel *owner;
- /** Group of links corresponding to this robot (if any) */
- std::string tag;
-
};
KinematicModel(void)
@@ -249,13 +250,14 @@
delete m_robots[i];
}
- virtual void build(robot_desc::URDF &model, const char *group = NULL);
+ virtual void build(robot_desc::URDF &model);
void setVerbose(bool verbose);
unsigned int getRobotCount(void) const;
Robot* getRobot(unsigned int index) const;
void getGroups(std::vector<std::string> &groups) const;
int getGroupID(const std::string &group) const;
+ Link* getLink(const std::string &link) const;
void computeTransforms(const double *params, int groupID = -1);
void printModelInfo(FILE *out = stdout) const;
@@ -286,6 +288,7 @@
std::vector<Robot*> m_robots;
std::vector<std::string> m_groups;
std::map<std::string, int> m_groupsMap;
+ std::map<std::string, Link*> m_linkMap;
bool m_verbose;
bool m_built;
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-08-01 21:17:02 UTC (rev 2462)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-08-01 21:17:18 UTC (rev 2463)
@@ -37,30 +37,23 @@
void planning_models::KinematicModel::Robot::computeTransforms(const double *params, int groupID)
{
- if (groupID >= 0)
- chain->computeTransform(params, groupID);
- else
- {
+ if (chain->robotRoot)
chain->globalTrans = owner->rootTransform;
- chain->computeTransform(params, groupID);
- }
+ chain->computeTransform(params, groupID);
}
void planning_models::KinematicModel::computeTransforms(const double *params, int groupID)
{
- if (groupID >= 0)
- for (unsigned int i = 0 ; i < groupChainStart[groupID].size() ; ++i)
- {
- groupChainStart[groupID][i]->computeTransform(params, groupID);
- params += groupStateIndexList[groupID].size();
- }
- else
- for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
- {
- m_robots[i]->chain->globalTrans = rootTransform;
- m_robots[i]->chain->computeTransform(params, groupID);
- params += m_robots[i]->stateDimension;
- }
+ unsigned int n = groupID >= 0 ? groupChainStart[groupID].size() : m_robots.size();
+
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ Joint *start = groupID >= 0 ? groupChainStart[groupID][i] : m_robots[i]->chain;
+ if (start->robotRoot)
+ start->globalTrans = rootTransform;
+ start->computeTransform(params, groupID);
+ params += groupID >= 0 ? groupStateIndexList[groupID].size() : m_robots[i]->stateDimension;
+ }
}
// we can optimize things here... (when we use identity transforms, for example)
@@ -127,7 +120,7 @@
m_groupsMap[m_groups[i]] = i;
}
-void planning_models::KinematicModel::build(robot_desc::URDF &model, const char *group)
+void planning_models::KinematicModel::build(robot_desc::URDF &model)
{
if (m_built)
{
@@ -142,43 +135,17 @@
groupStateIndexList.resize(m_groups.size());
groupChainStart.resize(m_groups.size());
- if (group)
+ for (unsigned int i = 0 ; i < model.getDisjointPartCount() ; ++i)
{
- robot_desc::URDF::Group *g = model.getGroup(group);
- if (g)
- {
- if (g->hasFlag("plan"))
- for (unsigned int i = 0 ; i < g->linkRoots.size() ; ++i)
- {
- robot_desc::URDF::Link *link = g->linkRoots[i];
- Robot *rb = new Robot(this);
- rb->groupStateIndexList.resize(m_groups.size());
- rb->groupChainStart.resize(m_groups.size());
- rb->tag = g->name;
- rb->chain = new Joint();
- buildChainJ(rb, NULL, rb->chain, link, model);
- m_robots.push_back(rb);
- }
- else
- fprintf(stderr, "Group '%s' is not marked for planning ('plan' flag).\n", group);
- }
- else
- fprintf(stderr, "Group '%s' not found.\n", group);
+ robot_desc::URDF::Link *link = model.getDisjointPart(i);
+ Robot *rb = new Robot(this);
+ rb->groupStateIndexList.resize(m_groups.size());
+ rb->groupChainStart.resize(m_groups.size());
+ rb->chain = new Joint();
+ buildChainJ(rb, NULL, rb->chain, link, model);
+ m_robots.push_back(rb);
}
- else
- {
- for (unsigned int i = 0 ; i < model.getDisjointPartCount() ; ++i)
- {
- robot_desc::URDF::Link *link = model.getDisjointPart(i);
- Robot *rb = new Robot(this);
- rb->groupStateIndexList.resize(m_groups.size());
- rb->groupChainStart.resize(m_groups.size());
- rb->chain = new Joint();
- buildChainJ(rb, NULL, rb->chain, link, model);
- m_robots.push_back(rb);
- }
- }
-
+
for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
{
/* copy state bounds */
@@ -202,6 +169,9 @@
groupStateIndexList[j].push_back(stateDimension + m_robots[i]->groupStateIndexList[j][k]);
stateDimension += m_robots[i]->stateDimension;
+
+ for (unsigned int j = 0 ; j < m_robots[i]->links.size() ; ++j)
+ m_linkMap[m_robots[i]->links[j]->name] = m_robots[i]->links[j];
}
}
@@ -226,11 +196,20 @@
return m_robots[index];
}
+planning_models::KinematicModel::Link* planning_models::KinematicModel::getLink(const std::string &link) const
+{
+ std::map<std::string, Link*>::const_iterator pos = m_linkMap.find(link);
+ return pos == m_linkMap.end() ? NULL : pos->second;
+}
+
void planning_models::KinematicModel::buildChainJ(Robot *robot, Link *parent, Joint* joint, robot_desc::URDF::Link* urdfLink, robot_desc::URDF &model)
{
joint->before = parent;
joint->after = new Link();
+ if (model.isRoot(urdfLink))
+ joint->robotRoot = true;
+
/* copy relevant data */
joint->limit[0] = urdfLink->joint->limit[0];
joint->limit[1] = urdfLink->joint->limit[1];
@@ -354,19 +333,6 @@
for (unsigned int i = 0 ; i < urdfLink->children.size() ; ++i)
{
- /* if building from a group of links, make sure we do not exit the group */
- if (!robot->tag.empty())
- {
- bool found = false;
- for (unsigned int k = 0 ; k < urdfLink->children[i]->groups.size() ; ++k)
- if (urdfLink->children[i]->groups[k]->name == robot->tag)
- {
- found = true;
- break;
- }
- if (!found)
- continue;
- }
Joint *newJoint = new Joint();
buildChainJ(robot, link, newJoint, urdfLink->children[i], model);
link->after.push_back(newJoint);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2008-08-02 00:12:15
|
Revision: 2478
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2478&view=rev
Author: sfkwc
Date: 2008-08-02 00:12:24 +0000 (Sat, 02 Aug 2008)
Log Message:
-----------
fixed bad manifest files
Modified Paths:
--------------
pkg/trunk/drivers/input/joy_view/manifest.xml
pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/manifest.xml
pkg/trunk/vision/gmmseg/manifest.xml
Modified: pkg/trunk/drivers/input/joy_view/manifest.xml
===================================================================
--- pkg/trunk/drivers/input/joy_view/manifest.xml 2008-08-01 23:36:32 UTC (rev 2477)
+++ pkg/trunk/drivers/input/joy_view/manifest.xml 2008-08-02 00:12:24 UTC (rev 2478)
@@ -1,4 +1,6 @@
<package>
+ <author>Morgan Quigley</author>
+ <license>BSD</license>
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="joy"/>
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2008-08-01 23:36:32 UTC (rev 2477)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2008-08-02 00:12:24 UTC (rev 2478)
@@ -1,4 +1,4 @@
-<manifest>
+<package>
<description>
A ROS package which is mainly just a wrapper of the outstanding sicktoolbox
library for interfacing with the SICK LMS2xx lasers.
@@ -10,4 +10,4 @@
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="rosTF"/>
-</manifest>
+</package>
Modified: pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/manifest.xml
===================================================================
--- pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/manifest.xml 2008-08-01 23:36:32 UTC (rev 2477)
+++ pkg/trunk/vision/camera_grab_samples/bumblebee_grab_sample/manifest.xml 2008-08-02 00:12:24 UTC (rev 2478)
@@ -1,4 +1,4 @@
-<manifest>
+<package>
<author>Morgan Quigley mqu...@cs...</author>
<description>
Shows how to grab stereo clouds from my Bumblebee daemon which is running
@@ -12,5 +12,5 @@
<depend package="roscpp"/>
<depend package="image_utils"/>
<depend package="bumblebee_bridge"/>
-</manifest>
+</package>
Modified: pkg/trunk/vision/gmmseg/manifest.xml
===================================================================
--- pkg/trunk/vision/gmmseg/manifest.xml 2008-08-01 23:36:32 UTC (rev 2477)
+++ pkg/trunk/vision/gmmseg/manifest.xml 2008-08-02 00:12:24 UTC (rev 2478)
@@ -2,8 +2,7 @@
<description>
Gaussian texture segmentation assuming object is in center of image.
</description>
- <author>Charles C. Kemp</author>
- <author>Hai Nguyen</author>
+ <author>Charles C. Kemp, Hai Nguyen</author>
<license>BSD</license>
<depend package="opencv"/>
<depend package="numpy"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-08-02 00:55:42
|
Revision: 2483
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2483&view=rev
Author: gerkey
Date: 2008-08-02 00:55:51 +0000 (Sat, 02 Aug 2008)
Log Message:
-----------
Rearranged modeling components into world_models.
Added sketch interface for costmap_2d.
Added Paths:
-----------
pkg/trunk/world_models/
pkg/trunk/world_models/costmap_2d/
pkg/trunk/world_models/costmap_2d/include/
pkg/trunk/world_models/costmap_2d/include/costmap_2d/
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/manifest.xml
pkg/trunk/world_models/map_server/
pkg/trunk/world_models/world_3d_map/
Removed Paths:
-------------
pkg/trunk/mapping/world_3d_map/
pkg/trunk/nav/map_server/
pkg/trunk/nav/sim/
Added: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h (rev 0)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-08-02 00:55:51 UTC (rev 2483)
@@ -0,0 +1,120 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: E. Gil Jones
+ */
+
+#ifndef COSTMAP_2D_COSTMAP_2D_H
+#define COSTMAP_2D_COSTMAP_2D_H
+
+/**
+
+@mainpage
+
+@htmlinclude manifest.html
+
+This library provides functionality for maintaining a 2D cost map. The primary inputs are:
+ - a priori 2-D occupancy grid map
+ - obstacle data
+This library is initialized with the a priori map, and thereafter updated
+with obstacle data. A sliding window model of persistence is used, in which
+obstacle data lives in the map for a fixed (but configurable) amount of time
+before being discarded. The static map lives forever.
+
+*/
+
+// For time support
+#include "ros/time.h"
+
+// For point clouds
+#include "std_msgs/PointCloudFloat32.h"
+
+// A bunch of x,y points, with a timestamp
+typedef struct
+{
+ double* pts;
+ size_t pts_num;
+ double ts;
+} ObstaclePts;
+
+class CostMap2D
+{
+ public:
+ /**
+ * @brief Constructor.
+ *
+ * @param window_length how long to hold onto obstacle data [sec]
+ */
+ CostMap2D(double window_length);
+
+ /**
+ * @brief Destructor.
+ */
+ ~CostMap2D();
+
+ /**
+ * @brief Initialize with a static map.
+ *
+ * @param width width of map [cells]
+ * @param height height of map [cells]
+ * @param resolution resolution of map [m/cell]
+ * @data row-major obstacle data, each value in the range [0,100] to
+ * indicate probability of occupancy, -1 to indicate no
+ * information
+ */
+ void setStaticMap(size_t width, size_t height,
+ double resolution, unsigned char* data);
+
+ /**
+ * @brief Add dynamic obstacles to the map.
+ *
+ * Updates the map to contain the new dynamic obstacles, in addition to
+ * any previously added obstacles (but only those that are still within
+ * the window_length).
+ */
+ void addObstacles(std_msg::PointCloudFloat32* cloud);
+
+ /**
+ * @brief A hook to allow time-based maintenance to occur (e.g.,
+ * removing stale obstacles, in the case that addObstacles() doesn't
+ * get called that often).
+ *
+ * @param ts current time
+ */
+ void update(ros::Time ts);
+
+ /**
+ * @brief Get pointer into the obstacle map (which contains both static
+ * and dynamic obstacles)
+ */
+ unsigned char* getMap();
+};
+
+
+
+#endif
Added: pkg/trunk/world_models/costmap_2d/manifest.xml
===================================================================
--- pkg/trunk/world_models/costmap_2d/manifest.xml (rev 0)
+++ pkg/trunk/world_models/costmap_2d/manifest.xml 2008-08-02 00:55:51 UTC (rev 2483)
@@ -0,0 +1,5 @@
+<package>
+<description>2D cost map maintenance library</description>
+<author>Willow Garage</author>
+<license>BSD</license>
+</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-08-02 22:10:25
|
Revision: 2492
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2492&view=rev
Author: hsujohnhsu
Date: 2008-08-02 22:10:25 +0000 (Sat, 02 Aug 2008)
Log Message:
-----------
updates to use new grippers.
Modified Paths:
--------------
pkg/trunk/clean_rosgazebo.scp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/clean_rosgazebo.scp
===================================================================
--- pkg/trunk/clean_rosgazebo.scp 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/clean_rosgazebo.scp 2008-08-02 22:10:25 UTC (rev 2492)
@@ -1,7 +1,8 @@
#!/bin/bash
#clean controllers
-(cd controllers/genericControllers ; rm -f lib/* ;make clean)
+(cd controllers/generic_controllers ; rm -f lib/* ;make clean)
+(cd deprecated/genericControllers ; rm -f lib/* ;make clean)
(cd controllers/pr2Controllers ; rm -f lib/* ;make clean)
#clean gazebo dependent stuff
@@ -13,7 +14,7 @@
(cd drivers/simulator/gazebo_sensors ; rm -f lib/* ;make clean)
(cd simulators/rosgazebo ; rm -f lib/* ;make clean)
-(cd robot/pr2_gazebo ; rm -f lib/* ;make clean)
+(cd robot_control_loops/pr2_gazebo ; rm -f lib/* ;make clean)
#clean 2dnav stack stuff
(cd nav/nav_view ; rm -f lib/* ;make clean)
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-08-02 22:10:25 UTC (rev 2492)
@@ -66,6 +66,7 @@
enum PR2_JOINT_TYPE{
PRISMATIC, ROTARY,
+ GRIPPER,
ROTARY_CONTINUOUS,
MAX_JOINT_TYPES
};
@@ -91,6 +92,7 @@
ARM_L_ELBOW_ROLL ,
ARM_L_WRIST_PITCH ,
ARM_L_WRIST_ROLL ,
+ ARM_L_GRIPPER_GAP , // added 20080802 by john
ARM_R_PAN ,
ARM_R_SHOULDER_PITCH,
ARM_R_SHOULDER_ROLL,
@@ -98,6 +100,7 @@
ARM_R_ELBOW_ROLL ,
ARM_R_WRIST_PITCH ,
ARM_R_WRIST_ROLL ,
+ ARM_R_GRIPPER_GAP , // added 20080802 by john
HEAD_YAW ,
HEAD_PITCH ,
HEAD_LASER_PITCH ,
@@ -298,7 +301,7 @@
// JointStart/JointEnd corresponds to the PR2_MODEL_ID, start and end id for each model
enum PR2_MODEL_ID {PR2_BASE ,PR2_SPINE ,PR2_LEFT_ARM ,PR2_RIGHT_ARM ,PR2_LEFT_GRIPPER ,PR2_RIGHT_GRIPPER ,HEAD ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ , MAX_MODELS };
const int JointStart[MAX_MODELS] = {CASTER_FL_STEER ,SPINE_ELEVATOR ,ARM_L_PAN ,ARM_R_PAN ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_YAW ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
- const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
+ const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_GRIPPER_GAP,ARM_R_GRIPPER_GAP,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
// Geometric description for the base
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-08-02 22:10:25 UTC (rev 2492)
@@ -102,6 +102,30 @@
// explicit damping coefficient for the joint
private: double dampCoef;
+ // get joints names from xml fields
+ private: std::string finger_l_name [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: std::string finger_tip_l_name [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: std::string finger_r_name [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: std::string finger_tip_r_name [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
+ // get the joints from parent
+ private: HingeJoint *finger_l_joint [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: HingeJoint *finger_tip_l_joint [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: HingeJoint *finger_r_joint [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: HingeJoint *finger_tip_r_joint [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
+ // assign pid for each finger for PD_CONTROL
+ private: controller::Pid *finger_l_pids [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: controller::Pid *finger_tip_l_pids [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: controller::Pid *finger_r_pids [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+ private: controller::Pid *finger_tip_r_pids [GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
+ // get name of each child, e.g. front_left_caster_steer
+ std::string actarrayName[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
+ // get type of each child, only check for special case for grippers for now. All others ignored. (TODO)
+ std::string actarrayType[GAZEBO_PR2ARRAY_MAX_NUM_ACTUATORS];
+
};
/** \} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Pr2_Actarray.cc 2008-08-02 21:34:41 UTC (rev 2491)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Pr2_Actarray.cc 2008-08-02 22:10:25 UTC (rev 2492)
@@ -115,80 +115,120 @@
SliderJoint *sjoint;
HingeJoint *hjoint;
-
// get children of the actarray, add to actuators object list
- int i =0;
- for (i=0, jNode = node->GetChild("joint"); jNode; i++)
+ int count =0;
+ for (jNode = node->GetChild("joint"); jNode ;)
{
// add each child
//actuators.AddJoint();
// get name of each child, e.g. front_left_caster_steer
- std::string name = jNode->GetString("name","",1);
+ actarrayName[count] = jNode->GetString("name","",1);
- // dynamic cast for type checking
- Joint* tmpJoint = dynamic_cast<Joint*>(this->myParent->GetJoint(name));
+ // get type of each child, only check for special case for grippers for now. All others ignored. (TODO)
+ actarrayType[count] = jNode->GetString("type","default",0); // FIXME: not mandatory for now
- // check what type of joint this is
- switch(tmpJoint->GetType())
+ if (actarrayType[count]=="gripper_special") // if this is a gripper, do something about it
{
- case Joint::SLIDER:
- // save joint in list
- this->joints[i] = tmpJoint;
- // set joint properties
- sjoint = dynamic_cast<SliderJoint*>( tmpJoint );
- // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
- this->myIface->data->actuators[i].actualPosition = sjoint->GetPosition();
- this->myIface->data->actuators[i].actualSpeed = sjoint->GetPositionRate();
- this->myIface->data->actuators[i].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
- this->myIface->data->actuators[i].jointType = PR2::PRISMATIC;
- break;
- case Joint::HINGE:
- // save joint in list
- this->joints[i] = tmpJoint;
- // set joint properties
- hjoint = dynamic_cast<HingeJoint*>( tmpJoint );
- // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
- this->myIface->data->actuators[i].actualPosition = hjoint->GetAngle();
- this->myIface->data->actuators[i].actualSpeed = hjoint->GetAngleRate();
- this->myIface->data->actuators[i].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
- this->myIface->data->actuators[i].jointType = PR2::ROTARY;
- break;
- case Joint::HINGE2:
- case Joint::BALL:
- case Joint::UNIVERSAL:
- gzthrow("Pr2_Actarray joint need to be either slider or hinge for now. Add support in Pr2_Actarray or talk to ");
- break;
+ std::cout << " gripper_special joint type " << actarrayType[count] << " " << jNode->GetString("type","default",0) << std::endl;
+ // get joints names from xml fields
+ finger_l_name [count] = jNode->GetString("left_proximal","",1);
+ finger_tip_l_name [count] = jNode->GetString("left_distal","",1);
+ finger_r_name [count] = jNode->GetString("right_proximal","",1);
+ finger_tip_r_name [count] = jNode->GetString("right_distal","",1);
+
+ // get the joints from parent, store the pointers to the joints locally in this class
+ finger_l_joint [count] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(finger_l_name [count]));
+ finger_tip_l_joint[count] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(finger_tip_l_name[count]));
+ finger_r_joint [count] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(finger_r_name [count]));
+ finger_tip_r_joint[count] = dynamic_cast<HingeJoint*>(this->myParent->GetJoint(finger_tip_r_name[count]));
+
+ // all four joints controlled by a single iface settings value
+ // IMPORTANT: in Iface, we'll use position as the gap command
+ // IMPORTANT: in Iface, if in torque control mode, pass through torque to all joints with the right sign
+ // given a single gap command, we can find out what the joint angles are
+ // FIXME: assuming for now that all joints are hinges
+
+ // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
+ //this->myIface->data->actuators[count].actualPosition = finger_l_joint[count]->GetAngle(); //TODO: get gap from joint positions
+ //this->myIface->data->actuators[count].actualSpeed = finger_l_joint[count]->GetAngleRate(); //TODO: get gap rate?
+ //this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ this->myIface->data->actuators[count].jointType = PR2::GRIPPER;
+
+ // create new pid for each finger for PD_CONTROL
+ this->finger_l_pids [count] = new Pid();
+ this->finger_r_pids [count] = new Pid();
+ this->finger_tip_l_pids[count] = new Pid();
+ this->finger_tip_r_pids[count] = new Pid();
}
+ else // if this is not a gripper, process them as sliders and hinges
+ {
+ std::cout << " default joint type " << actarrayType[count] << " " << jNode->GetString("type","default",0) << std::endl;
+ // dynamic cast for type checking
+ Joint* tmpJoint = dynamic_cast<Joint*>(this->myParent->GetJoint(actarrayName[count]));
- this->myIface->data->actuators[i].saturationTorque = jNode->GetDouble("saturationTorque",0.0,1);
- this->myIface->data->actuators[i].pGain = jNode->GetDouble("pGain" ,0.0,1);
- this->myIface->data->actuators[i].iGain = jNode->GetDouble("iGain" ,0.0,1);
- this->myIface->data->actuators[i].dGain = jNode->GetDouble("dGain" ,0.0,1);
- this->myIface->data->actuators[i].iClamp = jNode->GetDouble("iClamp",0.0,1);
- //this->myIface->data->actuators[i].cmdPosition = 0.0;
- //this->myIface->data->actuators[i].cmdSpeed = 0.0;
- std::string tmpControlMode = jNode->GetString("controlMode","PD_CONTROL",0);
- this->myIface->data->actuators[i].dampingCoefficient = jNode->GetDouble("explicitDampingCoefficient",0.0,0);
+ // check what type of joint this is
+ switch(tmpJoint->GetType())
+ {
+ case Joint::SLIDER:
+ // save joint in list
+ this->joints[count] = tmpJoint;
+ // set joint properties
+ sjoint = dynamic_cast<SliderJoint*>( tmpJoint );
+ // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
+ this->myIface->data->actuators[count].actualPosition = sjoint->GetPosition();
+ this->myIface->data->actuators[count].actualSpeed = sjoint->GetPositionRate();
+ this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ this->myIface->data->actuators[count].jointType = PR2::PRISMATIC;
+ break;
+ case Joint::HINGE:
+ // save joint in list
+ this->joints[count] = tmpJoint;
+ // set joint properties
+ hjoint = dynamic_cast<HingeJoint*>( tmpJoint );
+ // initialize some variables in gazebo.h for PR2, FIXME: re-evaluate, this is probably not necessary
+ this->myIface->data->actuators[count].actualPosition = hjoint->GetAngle();
+ this->myIface->data->actuators[count].actualSpeed = hjoint->GetAngleRate();
+ this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
+ this->myIface->data->actuators[count].jointType = PR2::ROTARY;
+ break;
+ case Joint::HINGE2:
+ case Joint::BALL:
+ case Joint::UNIVERSAL:
+ gzthrow("Pr2_Actarray joint need to be either slider or hinge for now. Add support in Pr2_Actarray or talk to ");
+ break;
+ }
+ }
+
+ // get additional information about this actuator
+ this->myIface->data->actuators[count].saturationTorque = jNode->GetDouble("saturationTorque",0.0,1);
+ this->myIface->data->actuators[count].pGain = jNode->GetDouble("pGain" ,0.0,1);
+ this->myIface->data->actuators[count].iGain = jNode->GetDouble("iGain" ,0.0,1);
+ this->myIface->data->actuators[count].dGain = jNode->GetDouble("dGain" ,0.0,1);
+ this->myIface->data->actuators[count].iClamp = jNode->GetDouble("iClamp",0.0,1);
+ std::string tmpControlMode = jNode->GetString("controlMode","PD_CONTROL",0);
+ this->myIface->data->actuators[count].dampingCoefficient = jNode->GetDouble("explicitDampingCoefficient",0.0,0);
+
// init a new pid for this joint
- this->pids[i] = new Pid();
+ this->pids[count] = new Pid();
// get time
- this->myIface->data->actuators[i].timestamp = Simulator::Instance()->GetSimTime();
+ this->myIface->data->actuators[count].timestamp = Simulator::Instance()->GetSimTime();
// set default control mode to:
if (tmpControlMode == "TORQUE_CONTROL")
- this->myIface->data->actuators[i].controlMode = PR2::TORQUE_CONTROL;
+ this->myIface->data->actuators[count].controlMode = PR2::TORQUE_CONTROL;
else if (tmpControlMode == "PD_TORQUE_CONTROL")
- this->myIface->data->actuators[i].controlMode = PR2::PD_TORQUE_CONTROL;
+ this->myIface->data->actuators[count].controlMode = PR2::PD_TORQUE_CONTROL;
else if (tmpControlMode == "PD_CONTROL")
- this->myIface->data->actuators[i].controlMode = PR2::PD_CONTROL;
+ this->myIface->data->actuators[count].controlMode = PR2::PD_CONTROL;
else
- this->myIface->data->actuators[i].controlMode = PR2::PD_CONTROL;
+ this->myIface->data->actuators[count].controlMode = PR2::PD_CONTROL;
jNode = jNode->GetNext("joint");
+ count++;
}
- this->num_joints = i;
+ this->num_joints = count;
}
////////////////////////////////////////////////////////////////////////////////
@@ -202,19 +242,39 @@
{
for (int count = 0; count < this->num_joints ; count++)
{
- // initialize pid stuff
- this->pids[count]->InitPid( this->myIface->data->actuators[count].pGain,
- this->myIface->data->actuators[count].iGain,
- this->myIface->data->actuators[count].dGain,
- this->myIface->data->actuators[count].iClamp,
- -this->myIface->data->actuators[count].iClamp
- );
- this->pids[count]->SetCurrentCmd(0);
- // as a first hack, initialize to zero velocity and saturation torque
- //this->joints[count]->SetParam( dParamVel , this->pids[count]->GetCurrentCmd());
- //this->joints[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
- this->joints[count]->SetParam( dParamVel , 0);
- this->joints[count]->SetParam( dParamFMax, 0);
+ if (actarrayType[count]=="gripper_special") // if this is a gripper, do something about it
+ {
+ // FIXME: temporary hack, initialize finger pid stuff, this should be based on transmissions
+ this->finger_l_pids[count] -> InitPid ( 1.0, 0.01, 0.0, 0.2, -0.2 );
+ this->finger_r_pids[count] -> InitPid ( 1.0, 0.01, 0.0, 0.2, -0.2 );
+ this->finger_tip_l_pids[count] -> InitPid ( 1.0, 0.01, 0.0, 0.2, -0.2 );
+ this->finger_tip_r_pids[count] -> InitPid ( 1.0, 0.01, 0.0, 0.2, -0.2 );
+ this->finger_l_joint[count] -> SetParam( dParamVel, 0 );
+ this->finger_r_joint[count] -> SetParam( dParamVel, 0 );
+ this->finger_tip_l_joint[count] -> SetParam( dParamVel, 0 );
+ this->finger_tip_r_joint[count] -> SetParam( dParamVel, 0 );
+ this->finger_l_joint[count] -> SetParam( dParamFMax, 0 );
+ this->finger_r_joint[count] -> SetParam( dParamFMax, 0 );
+ this->finger_tip_l_joint[count] -> SetParam( dParamFMax, 0 );
+ this->finger_tip_r_joint[count] -> SetParam( dParamFMax, 0 );
+ }
+ else
+ {
+ // initialize pid stuff
+ this->pids[count]->InitPid( this->myIface->data->actuators[count].pGain,
+ this->myIface->data->actuators[count].iGain,
+ this->myIface->data->actuators[count].dGain,
+ this->myIface->data->actuators[count].iClamp,
+ -this->myIface->data->actuators[count].iClamp
+ );
+ this->pids[count]->SetCurrentCmd(0);
+
+ // as a first hack, initialize to zero velocity and saturation torque
+ //this->joints[count]->SetParam( dParamVel , this->pids[count]->GetCurrentCmd());
+ //this->joints[count]->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
+ this->joints[count]->SetParam( dParamVel , 0);
+ this->joints[count]->SetParam( dParamFMax, 0);
+ }
}
lastTime = Simulator::Instance()->GetSimTime();
}
@@ -350,158 +410,280 @@
//////////////////////////////////////////////////////////////////////
for (int count = 0; count < this->num_joints; count++)
{
- switch(this->joints[count]->GetType())
+ if (actarrayType[count]=="gripper_special") // if this is a gripper, do something about it
{
- case Joint::SLIDER:
- sjoint = dynamic_cast<SliderJoint*>(this->joints[count]);
- cmdPosition = this->myIface->data->actuators[count].cmdPosition;
- cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
+ // get commands
+ cmdPosition = this->myIface->data->actuators[count].cmdPosition;
+ cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
+ // translate command position into joint angle
+ // command position is in radians
+ static double angleGapRatio = 0.2 / M_PI * 4.0;
+ double cmdGripperJointAngle = cmdPosition / angleGapRatio; // FIXME: TEMP HACK for transmission, 45 degrees with gripper gap command of 0.2 (meters)
- switch(this->myIface->data->actuators[count].controlMode)
- {
- case PR2::TORQUE_CONTROL :
- sjoint->SetSliderForce(this->myIface->data->actuators[count].cmdEffectorForce);
- break;
- // case PR2::PD_CONTROL1 :
- // // No fancy controller, just pass the commanded torque/force in (we are not modeling the motors for now)
- // positionError = cmdPosition - sjoint->GetPosition();
- // speedError = cmdSpeed - sjoint->GetPositionRate();
- // //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
- // currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
- // currentCmd = (currentCmd > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : currentCmd;
- // currentCmd = (currentCmd < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : currentCmd;
- // sjoint->SetSliderForce(currentCmd);
- // break;
- case PR2::PD_TORQUE_CONTROL :
- case PR2::PD_CONTROL : // velocity control
- //if (cmdPosition > sjoint->GetHighStop())
- // cmdPosition = sjoint->GetHighStop();
- //else if (cmdPosition < sjoint->GetLowStop())
- // cmdPosition = sjoint->GetLowStop();
-
- positionError = sjoint->GetPosition() - cmdPosition; //Error defined as actual - desired
- speedError = sjoint->GetPositionRate() - cmdSpeed;
- //std::cout << "slider e:" << speedError << " + " << positionError << std::endl;
- currentCmd = this->pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
-
- sjoint->SetParam( dParamVel , currentCmd );
- sjoint->SetParam( dParamFMax, this->myIface->data->actuators[count].saturationTorque );
- break;
- case PR2::SPEED_CONTROL :
- sjoint->SetParam( dParamVel, cmdSpeed);
- sjoint->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- break;
-
- default:
- break;
- }
+ // get control mode
+ switch(this->myIface->data->actuators[count].controlMode)
+ {
+ case PR2::TORQUE_CONTROL:
+ // EXPERIMENTAL: add explicit damping
+ /* ----------- finger_l ---------- */
+ currentRate = finger_l_joint[count]->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ //printf("Damping f %f v %f\n",dampForce,currentRate);
+ //std::cout<<"Force is:"<< this->myIface->data->actuators[count].cmdEffectorForce + dampForce<<std::endl;
+ // simply set torque
+ finger_l_joint[count]->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce + dampForce);
+ //std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
+ /* ----------- finger_tip_l ---------- */
+ currentRate = finger_tip_l_joint[count]->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ finger_tip_l_joint[count]->SetTorque(-(this->myIface->data->actuators[count].cmdEffectorForce + dampForce)); // negative enforces parallel
+ /* ----------- finger_r ---------- */
+ currentRate = finger_r_joint[count]->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ finger_r_joint[count]->SetTorque(-(this->myIface->data->actuators[count].cmdEffectorForce + dampForce)); // negative enforces symmetry
+ /* ----------- finger_tip_r ---------- */
+ currentRate = finger_tip_r_joint[count]->GetAngleRate();
+ dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
+ dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
+ dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
+ finger_tip_r_joint[count]->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce + dampForce);
+ break;
+ case PR2::PD_TORQUE_CONTROL :
+ break;
+ case PR2::SPEED_TORQUE_CONTROL :
+ break;
+ case PR2::PD_CONTROL:
+ // positive angle = open gripper
+ // no negative angle
+ /* ----------- finger_l ---------- */
+ currentAngle = finger_l_joint[count]->GetAngle();
+ currentRate = finger_l_joint[count]->GetAngleRate();
+ positionError = ModNPi2Pi(currentAngle - cmdGripperJointAngle);
+ speedError = currentRate - cmdSpeed;
+ currentCmd = this->finger_l_pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ finger_l_joint[count]->SetParam( dParamVel, currentCmd );
+ finger_l_joint[count]->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- this->myIface->data->actuators[count].actualPosition = sjoint->GetPosition();
- this->myIface->data->actuators[count].actualSpeed = sjoint->GetPositionRate();
- this->myIface->data->actuators[count].actualEffectorForce = 0.0; //TODO: use JointFeedback struct at some point
- break;
+ /* ----------- finger_tip_l ---------- */
+ currentAngle = finger_tip_l_joint[count]->GetAngle();
+ currentRate = finger_tip_l_joint[count]->GetAngleRate();
+ positionError = ModNPi2Pi(currentAngle + cmdGripperJointAngle); // negative command angle enforces parallel gripper
+ speedError = currentRate - cmdSpeed;
+ currentCmd = this->finger_tip_l_pids[count]->UpdatePid(positionError + 0.0*speedError, currentTime-this->lastTime);
+ finger_tip_l_joint[count]->SetParam( dParamVel, currentCmd );
+ finger_tip_l_joint[count]->SetParam( dParamFMax,this->myIface->data->actuators[count].saturationTorque );
- case Joint::HINGE:
- hjoint = dynamic_cast<HingeJoint*>(this->joints[count]);
- cmdPosition = this->myIface->data->actuators[count].cmdPosition;
- cmdSpeed = this->myIface->data->actuators[count].cmdSpeed;
- switch(this->myIface->data->actuators[count].controlMode)
- {
- case PR2::TORQUE_CONTROL:
- // TODO: EXPERIMENTAL: add explicit damping
- currentRate = hjoint->GetAngleRate();
- dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
- dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? this->myIface->data->actuators[count].saturationTorque : dampForce;
- dampForce = (dampForce < -this->myIface->data->actuators[count].saturationTorque ) ? -this->myIface->data->actuators[count].saturationTorque : dampForce;
- //printf("Damping f %f v %f\n",dampForce,currentRate);
- //std::cout<<"Force is:"<< this->myIface->data->actuators[count].cmdEffectorForce + dampForce<<std::endl;
- // simply set torque
- hjoint->SetTorque(this->myIface->data->actuators[count].cmdEffectorForce + dampForce);
- //std::cout << count << " " << this->myIface->data->actuators[count].controlMode << std::endl;
- break;
- case PR2::PD_TORQUE_CONTROL :
- // TODO: EXPERIMENTAL: add explicit damping
- currentRate = hjoint->GetAngleRate();
- dampForce = - this->myIface->data->actuators[count].dampingCoefficient * currentRate;
- dampForce = (dampForce > this->myIface->data->actuators[count].saturationTorque ) ? ...
[truncated message content] |
|
From: <rob...@us...> - 2008-08-04 18:06:43
|
Revision: 2505
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2505&view=rev
Author: rob_wheeler
Date: 2008-08-04 18:06:43 +0000 (Mon, 04 Aug 2008)
Log Message:
-----------
Move rosControllers to deprecated
Added Paths:
-----------
pkg/trunk/deprecated/rosControllers/
Removed Paths:
-------------
pkg/trunk/controllers/rosControllers/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-04 19:57:56
|
Revision: 2509
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2509&view=rev
Author: isucan
Date: 2008-08-04 19:57:51 +0000 (Mon, 04 Aug 2008)
Log Message:
-----------
updated messages for motion planning (not yet final)
Modified Paths:
--------------
pkg/trunk/messages/robot_msgs/msg/KinematicPath.msg
pkg/trunk/messages/robot_msgs/msg/KinematicState.msg
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
Added Paths:
-----------
pkg/trunk/messages/robot_msgs/msg/KinematicConstraint.msg
Added: pkg/trunk/messages/robot_msgs/msg/KinematicConstraint.msg
===================================================================
--- pkg/trunk/messages/robot_msgs/msg/KinematicConstraint.msg (rev 0)
+++ pkg/trunk/messages/robot_msgs/msg/KinematicConstraint.msg 2008-08-04 19:57:51 UTC (rev 2509)
@@ -0,0 +1,22 @@
+# This message contains the definition of a motion planning constraint.
+# Since there are multiple types of constraints, the 'type' member is used
+# to identify the different constraints
+
+
+# Value that identifies the type of constraint
+byte type
+
+
+##########################################
+# For constraints of type 0 #
+# (position of center of a certain body) #
+##########################################
+
+# The name of the body (link) for which the constraint is defined
+string body
+
+# The position of the body
+std_msgs/Point3DFloat64 position
+
+# The tolerance for this position
+float64 tolerance
Modified: pkg/trunk/messages/robot_msgs/msg/KinematicPath.msg
===================================================================
--- pkg/trunk/messages/robot_msgs/msg/KinematicPath.msg 2008-08-04 19:50:39 UTC (rev 2508)
+++ pkg/trunk/messages/robot_msgs/msg/KinematicPath.msg 2008-08-04 19:57:51 UTC (rev 2509)
@@ -1 +1,3 @@
+# The definition of a kinematic path. Simply a list of states
+
KinematicState[] states
Modified: pkg/trunk/messages/robot_msgs/msg/KinematicState.msg
===================================================================
--- pkg/trunk/messages/robot_msgs/msg/KinematicState.msg 2008-08-04 19:50:39 UTC (rev 2508)
+++ pkg/trunk/messages/robot_msgs/msg/KinematicState.msg 2008-08-04 19:57:51 UTC (rev 2509)
@@ -1 +1,4 @@
+# The definition of a kinematic state. Simply a list of double
+# precision floating point values
+
float64[] vals
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-04 19:50:39 UTC (rev 2508)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-04 19:57:51 UTC (rev 2509)
@@ -268,7 +268,7 @@
goal->state = new ompl::SpaceInformationKinematic::StateKinematic(dim);
for (int i = 0 ; i < dim ; ++i)
goal->state->values[i] = req.goal_state.vals[i];
- goal->threshold = 1e-6;
+ goal->threshold = req.threshold;
p.si->setGoal(goal);
printf("=======================================\n");
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-04 19:50:39 UTC (rev 2508)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-04 19:57:51 UTC (rev 2509)
@@ -49,6 +49,8 @@
robot_srvs::KinematicMotionPlan::response res;
req.model_id = "pr2::base+arms";
+ req.threshold = 1e-6;
+ req.distance_metric = "L2Square";
req.start_state.set_vals_size(34);
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
Modified: pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
===================================================================
--- pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-08-04 19:50:39 UTC (rev 2508)
+++ pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-08-04 19:57:51 UTC (rev 2509)
@@ -1,8 +1,63 @@
+# This message contains the definition for a request to the motion
+# planner
+
+
+
+# The model (defined in pr2.xml as a group) for which the motion
+# planner should plan for
string model_id
+
+
+# The starting state for the robot. This is eth complete state of the
+# robot, all joint values, everything that needs to be specified to
+# completely define where each part of the robot is in space. The
+# meaning for each element in the state vector can be extracted from
+# viewing the output of the robot model construction (the kinematic
+# model constructed from the parsed URDF model) in verbose mode
robot_msgs/KinematicState start_state
+
+
+# The goal state for the model to plan for. The dimension of this
+# state must be equal to the dimension of the state space
+# characterizing the model (group) to plan for. If this state has
+# dimension 0, it is assumed that the first state that satisfies the
+# constraints is correct
robot_msgs/KinematicState goal_state
+
+
+# The constraints the motion planner should respect
+robot_msgs/KinematicConstraint[] constraints
+
+
+# The maximum amount of time the motion planner is allowed to plan for
float64 allowed_time
+
+
+# A string that identifies which distance metric the planner should use
+string distance_metric
+
+
+# The threshold (distance) that is allowed between the returned
+# solution and the actual goal
+float64 threshold
+
+
+# Lower coordinate for a box defining the volume in the workspace the
+# motion planner is active in. If planning in 2D, only first 2 values
+# (x, y) of the points are used.
std_msgs/Point3DFloat64 volumeMin
+
+
+# Higher coordinate for the box described above
std_msgs/Point3DFloat64 volumeMax
+
---
+
+# The result of a motion plan: a kinematic path. If the motion plan is
+# not succesful, this path has 0 states. If the motion plan is
+# succesful, this path contains the minimum number of states (but it
+# includes start and goal states) to define the motions for the
+# robot. If more intermediate states are needed, linear interpolation
+# is to be assumed.
+
robot_msgs/KinematicPath path
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-04 23:34:04
|
Revision: 2522
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2522&view=rev
Author: isucan
Date: 2008-08-04 23:34:10 +0000 (Mon, 04 Aug 2008)
Log Message:
-----------
removed MIN, MAX (defined in <algorithm>), renamed CLAMP to clamp and made it a template (with Tully's help)
Modified Paths:
--------------
pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
pkg/trunk/util/math_utils/include/math_utils/angles.h
pkg/trunk/util/math_utils/include/math_utils/math_utils.h
pkg/trunk/util/math_utils/test/math_utils_unittest.cpp
Modified: pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp 2008-08-04 23:34:10 UTC (rev 2522)
@@ -129,9 +129,9 @@
void BaseController::receiveBaseCommandMessage(){
maxXDot = maxYDot = maxYawDot = 1; //Until we start reading the xml file for parameters
- double vx = CLAMP(baseCommandMessage.axes[1], -maxXDot, maxXDot);
- double vy = CLAMP(baseCommandMessage.axes[0], -maxYDot, maxYDot);
- double vyaw = CLAMP(baseCommandMessage.axes[2], -maxYawDot, maxYawDot);
+ double vx = math_utils::clamp<double>(baseCommandMessage.axes[1], -maxXDot, maxXDot);
+ double vy = math_utils::clamp<double>(baseCommandMessage.axes[0], -maxYDot, maxYDot);
+ double vyaw = math_utils::clamp<double>(baseCommandMessage.axes[2], -maxYawDot, maxYawDot);
setVelocity(vx, vy, vyaw);
}
Modified: pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-08-04 23:34:10 UTC (rev 2522)
@@ -28,8 +28,8 @@
*/
#include "laser_scan_utils/laser_scan.h"
+#include <algorithm>
-
namespace laser_scan{
@@ -133,7 +133,7 @@
temp_scan_ = scan_in; //HACK to store all metadata
/** \todo check for length of intensities too */
- unsigned int iterations = MIN(scan_in.ranges_size, num_ranges_);
+ unsigned int iterations = std::min(scan_in.ranges_size, num_ranges_);
for (unsigned int index = 0; index < iterations; index ++)
{
range_data_(current_packet_num_+1, index+1)= (double) scan_in.ranges[index];
@@ -158,7 +158,7 @@
NEWMAT::ColumnVector iColumn;
- unsigned int iterations = MIN(scan_result.ranges_size, num_ranges_);
+ unsigned int iterations = std::min(scan_result.ranges_size, num_ranges_);
/** \todo Resize output cloud/check length */
for (unsigned int index = 0; index < iterations; index ++)
{
Modified: pkg/trunk/util/math_utils/include/math_utils/angles.h
===================================================================
--- pkg/trunk/util/math_utils/include/math_utils/angles.h 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/util/math_utils/include/math_utils/angles.h 2008-08-04 23:34:10 UTC (rev 2522)
@@ -1,3 +1,37 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
#ifndef MATH_UTILS_ANGLES
#define MATH_UTILS_ANGLES
@@ -3,68 +37,73 @@
#include <cmath>
-/** Convert degrees to radians */
-static inline double from_degrees(double degrees)
+namespace math_utils
{
- return degrees * M_PI / 180.0;
-}
-
-/** Convert radians to degrees */
-static inline double to_degrees(double radians)
-{
- return radians * 180.0 / M_PI;
-}
-
-/*
- * normalize_angle_positive
- *
- * Normalizes the angle to be 0 circle to 1 circle
- * It takes and returns native units.
- */
-
-static inline double normalize_angle_positive(double angle)
-{
- return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
-}
-/*
- * normalize
- *
- * Normalizes the angle to be -1/2 circle to +1/2 circle
- * It takes and returns native units.
- *
- */
-
-static inline double normalize_angle(double angle)
-{
- double a = normalize_angle_positive(angle);
- if (a > M_PI)
- a -= 2.0 *M_PI;
- return a;
-}
-
-/*
- * shortest_angular_distance
- *
- * Given 2 angles, this returns the shortest angular
- * difference. The inputs and ouputs are of course native
- * units.
- *
- * As an example, if native units are degrees, the result
- * would always be -180 <= result <= 180. Adding the result
- * to "from" will always get you an equivelent angle to "to".
- */
-
-static inline double shortest_angular_distance(double from, double to)
-{
- double result = normalize_angle_positive(normalize_angle_positive(to) - normalize_angle_positive(from));
-
- if (result > M_PI)
- // If the result > 180,
- // It's shorter the other way.
- result = -(2.0*M_PI - result);
- return normalize_angle(result);
+ /** Convert degrees to radians */
+
+ static inline double from_degrees(double degrees)
+ {
+ return degrees * M_PI / 180.0;
+ }
+
+ /** Convert radians to degrees */
+ static inline double to_degrees(double radians)
+ {
+ return radians * 180.0 / M_PI;
+ }
+
+ /*
+ * normalize_angle_positive
+ *
+ * Normalizes the angle to be 0 circle to 1 circle
+ * It takes and returns native units.
+ */
+
+ static inline double normalize_angle_positive(double angle)
+ {
+ return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
+ }
+ /*
+ * normalize
+ *
+ * Normalizes the angle to be -1/2 circle to +1/2 circle
+ * It takes and returns native units.
+ *
+ */
+
+ static inline double normalize_angle(double angle)
+ {
+ double a = normalize_angle_positive(angle);
+ if (a > M_PI)
+ a -= 2.0 *M_PI;
+ return a;
+ }
+
+ /*
+ * shortest_angular_distance
+ *
+ * Given 2 angles, this returns the shortest angular
+ * difference. The inputs and ouputs are of course native
+ * units.
+ *
+ * As an example, if native units are degrees, the result
+ * would always be -180 <= result <= 180. Adding the result
+ * to "from" will always get you an equivelent angle to "to".
+ */
+
+ static inline double shortest_angular_distance(double from, double to)
+ {
+ double result = normalize_angle_positive(normalize_angle_positive(to) - normalize_angle_positive(from));
+
+ if (result > M_PI)
+ // If the result > 180,
+ // It's shorter the other way.
+ result = -(2.0*M_PI - result);
+
+ return normalize_angle(result);
+ }
+
}
-
#endif
Modified: pkg/trunk/util/math_utils/include/math_utils/math_utils.h
===================================================================
--- pkg/trunk/util/math_utils/include/math_utils/math_utils.h 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/util/math_utils/include/math_utils/math_utils.h 2008-08-04 23:34:10 UTC (rev 2522)
@@ -1,22 +1,51 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
#ifndef MATH_UTIL_H
#define MATH_UTIL_H
-/** Minimum between two doubles */
-static inline double MIN(double a, double b)
-{
- return a < b ? a : b;
-}
+#include <algorithm>
-/** Maximum between two doubles */
-static inline double MAX(double a, double b)
+namespace math_utils
{
- return a > b ? a : b;
+
+ /** Clamp value a between b and c */
+ template<typename T>
+ static inline const T& clamp(const T &a, const T &b, const T &c)
+ {
+ return std::min<T>(std::max<T>(b, a), c);
+ }
}
-/** Clamp value a between b and c */
-static inline double CLAMP(double a, double b, double c)
-{
- return MIN(MAX(b, a), c);
-}
-
#endif
Modified: pkg/trunk/util/math_utils/test/math_utils_unittest.cpp
===================================================================
--- pkg/trunk/util/math_utils/test/math_utils_unittest.cpp 2008-08-04 23:06:06 UTC (rev 2521)
+++ pkg/trunk/util/math_utils/test/math_utils_unittest.cpp 2008-08-04 23:34:10 UTC (rev 2522)
@@ -37,17 +37,11 @@
}
TEST(math_utils, BasicOperations){
- EXPECT_EQ(MIN(10, 20), 10);
- EXPECT_EQ(MAX(10, 20), 20);
- EXPECT_EQ(CLAMP(-10, 10, 20), 10);
- EXPECT_EQ(CLAMP(15, 10, 20), 15);
- EXPECT_EQ(CLAMP(25, 10, 20), 20);
+ EXPECT_EQ(math_utils::clamp<int>(-10, 10, 20), 10);
+ EXPECT_EQ(math_utils::clamp<int>(15, 10, 20), 15);
+ EXPECT_EQ(math_utils::clamp<int>(25, 10, 20), 20);
}
-TEST(math_utils, BoundaryCases){
- EXPECT_EQ(MAX(10, 10), 10);
-}
-
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-05 04:11:55
|
Revision: 2544
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2544&view=rev
Author: isucan
Date: 2008-08-05 04:12:01 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
code for checking whether a point is inside a box or a cylinder
Modified Paths:
--------------
pkg/trunk/util/collision_space/include/collision_space/environment.h
pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/world_3d_map/manifest.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/util/collision_space/include/collision_space/util.h
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-08-05 03:59:39 UTC (rev 2543)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-08-05 04:12:01 UTC (rev 2544)
@@ -32,8 +32,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#ifndef KINEMATIC_ENVIRONMENT_MODEL_
-#define KINEMATIC_ENVIRONMENT_MODEL_
+#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_
+#define COLLISION_SPACE_ENVIRONMENT_MODEL_
#include <planning_models/kinematic.h>
#include <rosthread/mutex.h>
Modified: pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-08-05 03:59:39 UTC (rev 2543)
+++ pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-08-05 04:12:01 UTC (rev 2544)
@@ -32,8 +32,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#ifndef KINEMATIC_ENVIRONMENT_MODEL_ODE_
-#define KINEMATIC_ENVIRONMENT_MODEL_ODE_
+#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_ODE_
+#define COLLISION_SPACE_ENVIRONMENT_MODEL_ODE_
#include <collision_space/environment.h>
#include <ode/ode.h>
Added: pkg/trunk/util/collision_space/include/collision_space/util.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/util.h (rev 0)
+++ pkg/trunk/util/collision_space/include/collision_space/util.h 2008-08-05 04:12:01 UTC (rev 2544)
@@ -0,0 +1,186 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef COLLISION_SPACE_UTIL_
+#define COLLISION_SPACE_UTIL_
+
+#include <libTF/Pose3D.h>
+
+namespace collision_space
+{
+
+ class Object
+ {
+ public:
+ Object(void)
+ {
+ m_scale = 1.0;
+ }
+
+ virtual ~Object(void)
+ {
+ }
+
+ void setScale(double scale)
+ {
+ m_scale = scale;
+ }
+
+ bool containsPoint(const libTF::Pose3D &pose, const libTF::Pose3D::Position &p) const
+ {
+ /* bring point in the reference frame described by pose */
+ libTF::Pose3D::Position pt = p;
+ pose.applyToPosition(pt);
+
+ /* since the body is centered at origin, scaling the body is equivalent
+ * to scaling the coordinates of the point */
+ pt.x *= m_scale;
+ pt.y *= m_scale;
+ pt.z *= m_scale;
+
+ return containsPoint(pt);
+ }
+
+ virtual bool containsPoint(const libTF::Pose3D::Position &p) const = 0;
+
+ protected:
+
+ double m_scale;
+
+ };
+
+ class Sphere : public Object
+ {
+ public:
+ Sphere(double radius = 0) : Object()
+ {
+ setRadius(radius);
+ }
+
+ virtual ~Sphere(void)
+ {
+ }
+
+ void setRadius(double radius)
+ {
+ m_radius = radius;
+ m_radius2 = radius * radius;
+ }
+
+ virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ {
+ return p.x * p.x + p.y * p.y + p.z * p.z < m_radius2;
+ }
+
+ protected:
+
+ double m_radius2;
+ double m_radius;
+ };
+
+ class Cylinder : public Object
+ {
+ public:
+ Cylinder(double length = 0.0, double radius = 0.0) : Object()
+ {
+ setDimensions(length, radius);
+ }
+
+ virtual ~Cylinder(void)
+ {
+ }
+
+ void setDimensions(double length, double radius)
+ {
+ m_length = length;
+ m_length2 = length / 2.0;
+ m_radius = radius;
+ m_radius2 = radius * radius;
+ }
+
+ virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ {
+ if (fabs(p.z) > m_length2)
+ return false;
+ return p.x * p.x + p.y * p.y < m_radius2;
+ }
+
+ protected:
+
+ double m_length;
+ double m_length2;
+ double m_radius;
+ double m_radius2;
+ };
+
+ class Box : public Object
+ {
+ public:
+ Box(double length = 0.0, double width = 0.0, double height = 0.0) : Object()
+ {
+ setDimensions(length, width, height);
+ }
+
+ virtual ~Box(void)
+ {
+ }
+
+ void setDimensions(double length, double width, double height) // x, y, z
+ {
+ m_length = length;
+ m_length2 = length / 2.0;
+ m_width = width;
+ m_width2 = width / 2.0;
+ m_height = height;
+ m_height2 = height / 2.0;
+ }
+
+ virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ {
+ return fabs(p.x) < m_length2 && fabs(p.y) < m_width2 && fabs(p.z) < m_height2;
+ }
+
+ protected:
+ double m_length;
+ double m_width;
+ double m_height;
+ double m_length2;
+ double m_width2;
+ double m_height2;
+ };
+
+
+}
+
+#endif
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-05 03:59:39 UTC (rev 2543)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-05 04:12:01 UTC (rev 2544)
@@ -8,5 +8,5 @@
<depend package="xmlparam" />
<depend package="rosTF" />
<depend package="random_utils" />
-
+<depend package="collision_space" />
</package>
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 03:59:39 UTC (rev 2543)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 04:12:01 UTC (rev 2544)
@@ -90,6 +90,7 @@
#include <rostools/Log.h>
#include <rosTF/rosTF.h>
#include <random_utils/random_utils.h>
+#include <collision_space/util.h>
#include <deque>
#include <cmath>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-08-05 07:46:43
|
Revision: 2549
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2549&view=rev
Author: hsujohnhsu
Date: 2008-08-05 07:46:49 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
removed old gripper interface implementation. updated sensors in pr2.xml, for parsing only.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
pkg/trunk/robot_control_loops/pr2_gazebo/src/RosGazeboControlsNode.cpp
pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp
pkg/trunk/robot_control_loops/pr2_gazebo/src/test2.cpp
pkg/trunk/robot_control_loops/pr2_gazebo/src/test3.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/simulators/rosgazebo/src/rosgazebo.cc
pkg/trunk/simulators/rosgazebo/src/rosgazebowriter.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-08-05 07:46:49 UTC (rev 2549)
@@ -250,17 +250,6 @@
public: PR2_ERROR_CODE EnableHead();
/*! \fn
- \brief Enable the left gripper (i.e. enable all actuators corresponding to the left gripper)
- */
- public: PR2_ERROR_CODE EnableGripperLeft();
-
- /*! \fn
- \brief Enable the right gripper (i.e. enable all actuators corresponding to the right gripper)
- */
- public: PR2_ERROR_CODE EnableGripperRight();
-
-
- /*! \fn
\brief Enable the arm (i.e. enable all actuators corresponding to an arm)
\param id - model ID, see pr2Core.h for a list of model IDs
*/
@@ -268,13 +257,6 @@
/*! \fn
- \brief Enable the gripper (i.e. enable all actuators corresponding to the gripper)
- \param id - model ID, see pr2Core.h for a list of model IDs
- */
- public: PR2_ERROR_CODE EnableGripper(PR2_MODEL_ID id);
-
-
- /*! \fn
\brief Enable the base (i.e. enable all actuators corresponding to the base)
*/
public: PR2_ERROR_CODE EnableBase();
@@ -291,17 +273,6 @@
public: PR2_ERROR_CODE DisableHead();
/*! \fn
- \brief Disable the head (i.e. enable all actuators corresponding to the head)
- */
- public: PR2_ERROR_CODE DisableGripperLeft();
-
- /*! \fn
- \brief Disable the head (i.e. enable all actuators corresponding to the head)
- */
- public: PR2_ERROR_CODE DisableGripperRight();
-
-
- /*! \fn
\brief Disable the arm (i.e. disable all actuators corresponding to an arm)
\param id - model ID, see pr2Core.h for a list of model IDs
*/
@@ -309,13 +280,6 @@
/*! \fn
- \brief Disable the gripper (i.e. disable all actuators corresponding to a gripper)
- \param id - model ID, see pr2Core.h for a list of model IDs
- */
- public: PR2_ERROR_CODE DisableGripper(PR2_MODEL_ID id);
-
-
- /*! \fn
\brief Disable the base (i.e. disable all actuators corresponding to the base)
*/
public: PR2_ERROR_CODE DisableBase();
@@ -627,6 +591,9 @@
public: PR2_ERROR_CODE StopRobot();
+ /*! \fn
+ \brief - Shortcuts to get gripper commands
+ */
public: PR2_ERROR_CODE GetLeftGripperCmd(double *gap,double *force);
public: PR2_ERROR_CODE GetLeftGripperActual(double *gap,double *force);
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -41,40 +41,12 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::EnableGripper(PR2_MODEL_ID id)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- EnableGripperLeft();
- break;
- case PR2_RIGHT_GRIPPER:
- EnableGripperRight();
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2Robot::EnableHead()
{
hw.EnableModel(HEAD);
return PR2_ALL_OK;
}
-PR2_ERROR_CODE PR2Robot::EnableGripperLeft()
-{
- hw.EnableModel(PR2_LEFT_GRIPPER);
- return PR2_ALL_OK;
-}
-
-PR2_ERROR_CODE PR2Robot::EnableGripperRight()
-{
- hw.EnableModel(PR2_RIGHT_GRIPPER);
- return PR2_ALL_OK;
-}
-
PR2_ERROR_CODE PR2Robot::EnableBase()
{
hw.EnableModel(PR2_BASE);
@@ -103,40 +75,12 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::DisableGripper(PR2_MODEL_ID id)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- DisableGripperLeft();
- break;
- case PR2_RIGHT_GRIPPER:
- DisableGripperRight();
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2Robot::DisableHead()
{
hw.DisableModel(HEAD);
return PR2_ALL_OK;
}
-PR2_ERROR_CODE PR2Robot::DisableGripperLeft()
-{
- hw.DisableModel(PR2_LEFT_GRIPPER);
- return PR2_ALL_OK;
-}
-
-PR2_ERROR_CODE PR2Robot::DisableGripperRight()
-{
- hw.DisableModel(PR2_RIGHT_GRIPPER);
- return PR2_ALL_OK;
-}
-
PR2_ERROR_CODE PR2Robot::DisableBase()
{
hw.DisableModel(PR2_BASE);
@@ -927,25 +871,25 @@
PR2_ERROR_CODE PR2Robot::GetLeftGripperCmd(double *gap,double *force)
{
- hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
+ hw.GetJointServoCmd((PR2_JOINT_ID)PR2::ARM_L_GRIPPER_GAP,gap,force);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::GetLeftGripperActual(double *gap,double *force)
{
- hw.GetGripperActual((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
+ hw.GetJointServoActual((PR2_JOINT_ID)PR2::ARM_L_GRIPPER_GAP,gap,force);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::GetRightGripperCmd(double *gap,double *force)
{
- hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_RIGHT_GRIPPER,gap,force);
+ hw.GetJointServoCmd((PR2_JOINT_ID)PR2::ARM_R_GRIPPER_GAP,gap,force);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::GetRightGripperActual(double *gap,double *force)
{
- hw.GetGripperActual((PR2_MODEL_ID)PR2::PR2_RIGHT_GRIPPER,gap,force);
+ hw.GetJointServoActual((PR2_JOINT_ID)PR2::ARM_R_GRIPPER_GAP,gap,force);
return PR2_ALL_OK;
}
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -101,12 +101,12 @@
void close_gripper()
{
- myPR2.hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.05, 500);
+ myPR2.hw.SetJointServoCmd(PR2::ARM_R_GRIPPER_GAP, 0.0, 500);
}
void open_gripper()
{
- myPR2.hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.15, 500);
+ myPR2.hw.SetJointServoCmd(PR2::ARM_R_GRIPPER_GAP, 0.15, 500);
}
void go_down()
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-08-05 07:46:49 UTC (rev 2549)
@@ -260,34 +260,6 @@
double* simTime);
/*! \fn
- \brief - Open gripper
- */
- public: PR2_ERROR_CODE OpenGripper(PR2_MODEL_ID id,double gap, double force);
-
- /*! \fn
- \brief - Close gripper
- */
- public: PR2_ERROR_CODE CloseGripper(PR2_MODEL_ID id,double gap, double force);
-
- /*! \fn
- \brief - Get gripper gap and force setpoint
- */
- public: PR2_ERROR_CODE GetGripperCmd(PR2_MODEL_ID id,double *gap,double *force);
-
- /*! \fn
- \brief - Get gripper gap and force status
- */
- public: PR2_ERROR_CODE GetGripperActual(PR2_MODEL_ID id,double *gap,double *force);
-
- /*! \fn
- \brief - Set gripper p,i,d gains
- */
- public: PR2_ERROR_CODE SetGripperGains(PR2_MODEL_ID id,double p,double i, double d);
- /*!
- \brief - control mode for the arms, possible values are joint space control or cartesian space control
- */
-
- /*! \fn
\brief - Get camera data
*/
public: PR2_ERROR_CODE GetCameraImage(PR2_SENSOR_ID id ,
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -34,8 +34,6 @@
gazebo::Client *client;
gazebo::SimulationIface *simIface;
gazebo::PR2ArrayIface *pr2Iface;
-gazebo::PR2GripperIface *pr2GripperLeftIface;
-gazebo::PR2GripperIface *pr2GripperRightIface;
gazebo::LaserIface *pr2LaserIface;
gazebo::LaserIface *pr2BaseLaserIface;
gazebo::CameraIface *pr2CameraGlobalIface;
@@ -79,8 +77,6 @@
client = new gazebo::Client();
simIface = new gazebo::SimulationIface();
pr2Iface = new gazebo::PR2ArrayIface();
- pr2GripperLeftIface = new gazebo::PR2GripperIface();
- pr2GripperRightIface = new gazebo::PR2GripperIface();
pr2LaserIface = new gazebo::LaserIface();
pr2BaseLaserIface = new gazebo::LaserIface();
pr2CameraGlobalIface = new gazebo::CameraIface();
@@ -162,7 +158,7 @@
/// Open the wrist and forearm cameras
try
{
- pr2WristCameraLeftIface->Open(client, "wrist_cam_left_iface");
+ pr2WristCameraLeftIface->Open(client, "wrist_cam_left_iface_1");
}
catch (std::string e)
{
@@ -173,7 +169,7 @@
try
{
- pr2WristCameraRightIface->Open(client, "wrist_cam_right_iface");
+ pr2WristCameraRightIface->Open(client, "wrist_cam_right_iface_1");
}
catch (std::string e)
{
@@ -204,30 +200,6 @@
pr2ForearmCameraRightIface = NULL;
}
- /// Open the Position interface for gripper left
- try
- {
- pr2GripperLeftIface->Open(client, "pr2_gripper_left_iface");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 gripper left interface\n"
- << e << "\n";
- pr2GripperLeftIface = NULL;
- }
-
- /// Open the Position interface for gripper right
- try
- {
- pr2GripperRightIface->Open(client, "pr2_gripper_right_iface");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 gripper right interface\n"
- << e << "\n";
- pr2GripperRightIface = NULL;
- }
-
/// Open the laser interface for hokuyo
try
{
@@ -336,51 +308,16 @@
// fill in actuator data
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperLeftIface)
- {
- pr2GripperLeftIface->Lock(1);
- this->jointData[id].cmdEnableMotor = pr2GripperLeftIface->data->cmdEnableMotor ;
- this->jointData[id].controlMode = pr2GripperLeftIface->data->controlMode ;
- this->jointData[id].pGain = pr2GripperLeftIface->data->pGain ;
- this->jointData[id].iGain = pr2GripperLeftIface->data->iGain ;
- this->jointData[id].dGain = pr2GripperLeftIface->data->dGain ;
- this->jointData[id].cmdGap = pr2GripperLeftIface->data->cmdGap ;
- this->jointData[id].cmdEffectorForce = pr2GripperLeftIface->data->cmdForce ;
- this->jointData[id].cmdSpeed = pr2GripperLeftIface->data->cmdPositionRate ;
- pr2GripperLeftIface->Unlock();
- }
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperRightIface)
- {
- pr2GripperRightIface->Lock(1);
- this->jointData[id].cmdEnableMotor = pr2GripperRightIface->data->cmdEnableMotor ;
- this->jointData[id].controlMode = pr2GripperRightIface->data->controlMode ;
- this->jointData[id].pGain = pr2GripperRightIface->data->pGain ;
- this->jointData[id].iGain = pr2GripperRightIface->data->iGain ;
- this->jointData[id].dGain = pr2GripperRightIface->data->dGain ;
- this->jointData[id].cmdGap = pr2GripperRightIface->data->cmdGap ;
- this->jointData[id].cmdEffectorForce = pr2GripperRightIface->data->cmdForce ;
- this->jointData[id].cmdSpeed = pr2GripperRightIface->data->cmdPositionRate ;
- pr2GripperRightIface->Unlock();
- }
- }
- else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
- {
- pr2Iface->Lock(1);
- this->jointData[id].cmdEnableMotor = pr2Iface->data->actuators[id].cmdEnableMotor ;
- this->jointData[id].controlMode = pr2Iface->data->actuators[id].controlMode ;
- this->jointData[id].pGain = pr2Iface->data->actuators[id].pGain ;
- this->jointData[id].iGain = pr2Iface->data->actuators[id].iGain ;
- this->jointData[id].dGain = pr2Iface->data->actuators[id].dGain ;
- this->jointData[id].cmdPosition = pr2Iface->data->actuators[id].cmdPosition ;
- this->jointData[id].cmdSpeed = pr2Iface->data->actuators[id].cmdSpeed ;
- this->jointData[id].cmdEffectorForce = pr2Iface->data->actuators[id].cmdEffectorForce ;
- pr2Iface->Unlock();
- }
+ pr2Iface->Lock(1);
+ this->jointData[id].cmdEnableMotor = pr2Iface->data->actuators[id].cmdEnableMotor ;
+ this->jointData[id].controlMode = pr2Iface->data->actuators[id].controlMode ;
+ this->jointData[id].pGain = pr2Iface->data->actuators[id].pGain ;
+ this->jointData[id].iGain = pr2Iface->data->actuators[id].iGain ;
+ this->jointData[id].dGain = pr2Iface->data->actuators[id].dGain ;
+ this->jointData[id].cmdPosition = pr2Iface->data->actuators[id].cmdPosition ;
+ this->jointData[id].cmdSpeed = pr2Iface->data->actuators[id].cmdSpeed ;
+ this->jointData[id].cmdEffectorForce = pr2Iface->data->actuators[id].cmdEffectorForce ;
+ pr2Iface->Unlock();
}
@@ -655,103 +592,6 @@
}
};
-PR2_ERROR_CODE PR2HW::OpenGripper(PR2_MODEL_ID id,double gap,double force)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- this->jointData[ii].cmdMode = GAZEBO_PR2GRIPPER_CMD_OPEN;
- this->jointData[ii].cmdSpeed = 1.0;
- this->jointData[ii].cmdEffectorForce = force;
- this->jointData[ii].cmdGap = gap;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2HW::CloseGripper(PR2_MODEL_ID id,double gap,double force)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- std::cout << "JointStart " << JointStart[id] << " joint end " << JointEnd[id] << std::endl;
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- this->jointData[ii].cmdMode = GAZEBO_PR2GRIPPER_CMD_OPEN;
- this->jointData[ii].cmdSpeed = 1.0;
- this->jointData[ii].cmdEffectorForce = force;
- this->jointData[ii].cmdGap = gap;
- std::cout << "Setting gap for " << ii << " " << gap << std::endl;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2HW::GetGripperCmd(PR2_MODEL_ID id,double *gap,double *force)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- *force = this->jointData[ii].cmdEffectorForce;
- *gap = this->jointData[ii].cmdGap ;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2HW::GetGripperActual(PR2_MODEL_ID id,double *gap,double *force)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- *force = this->jointData[ii].actualEffectorForce;
- *gap = this->jointData[ii].actualGap ;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2HW::SetGripperGains(PR2_MODEL_ID id,double p,double i, double d)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- this->jointData[ii].pGain = p;
- this->jointData[ii].iGain = i;
- this->jointData[ii].dGain = d;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2HW::GetCameraImage(PR2_SENSOR_ID id ,
uint32_t* width ,uint32_t* height ,
uint32_t* depth ,
@@ -1061,56 +901,11 @@
// receive data from hardware
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperLeftIface)
- {
- pr2GripperLeftIface->Lock(1);
- this->jointData[id].actualPosition = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualGap = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualSpeed = ( pr2GripperLeftIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPositionRate[1]);
- this->jointData[id].actualSpeed = pr2GripperLeftIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
-
- this->jointData[id].actualEffectorForce = pr2GripperLeftIface->data->gripperForce;
- this->jointData[id].actualFingerPosition[0] = pr2GripperLeftIface->data->actualFingerPosition[0];
- this->jointData[id].actualFingerPosition[1] = pr2GripperLeftIface->data->actualFingerPosition[1];
- this->jointData[id].actualFingerPositionRate[0] = pr2GripperLeftIface->data->actualFingerPositionRate[0];
- this->jointData[id].actualFingerPositionRate[1] = pr2GripperLeftIface->data->actualFingerPositionRate[1];
- pr2GripperLeftIface->Unlock();
- }
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperRightIface)
- {
- pr2GripperRightIface->Lock(1);
- this->jointData[id].actualPosition = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualGap = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualSpeed = ( pr2GripperRightIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPositionRate[1]);
- this->jointData[id].actualSpeed = pr2GripperRightIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
-
- this->jointData[id].actualEffectorForce = pr2GripperRightIface->data->gripperForce;
- this->jointData[id].actualFingerPosition[0] = pr2GripperRightIface->data->actualFingerPosition[0];
- this->jointData[id].actualFingerPosition[1] = pr2GripperRightIface->data->actualFingerPosition[1];
- this->jointData[id].actualFingerPositionRate[0] = pr2GripperRightIface->data->actualFingerPositionRate[0];
- this->jointData[id].actualFingerPositionRate[1] = pr2GripperRightIface->data->actualFingerPositionRate[1];
- pr2GripperRightIface->Unlock();
- }
- }
- else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
- {
- pr2Iface->Lock(1);
- this->jointData[id].actualPosition = pr2Iface->data->actuators[id].actualPosition ;
- this->jointData[id].actualSpeed = pr2Iface->data->actuators[id].actualSpeed ;
- this->jointData[id].actualEffectorForce = pr2Iface->data->actuators[id].actualEffectorForce;
- pr2Iface->Unlock();
- }
+ pr2Iface->Lock(1);
+ this->jointData[id].actualPosition = pr2Iface->data->actuators[id].actualPosition ;
+ this->jointData[id].actualSpeed = pr2Iface->data->actuators[id].actualSpeed ;
+ this->jointData[id].actualEffectorForce = pr2Iface->data->actuators[id].actualEffectorForce;
+ pr2Iface->Unlock();
}
@@ -1118,51 +913,16 @@
// send commands to hardware
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperLeftIface)
- {
- pr2GripperLeftIface->Lock(1);
- pr2GripperLeftIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2GripperLeftIface->data->controlMode = this->jointData[id].controlMode ;
- pr2GripperLeftIface->data->pGain = this->jointData[id].pGain ;
- pr2GripperLeftIface->data->iGain = this->jointData[id].iGain ;
- pr2GripperLeftIface->data->dGain = this->jointData[id].dGain ;
- pr2GripperLeftIface->data->cmdGap = this->jointData[id].cmdGap ;
- pr2GripperLeftIface->data->cmdForce = this->jointData[id].cmdEffectorForce;
- pr2GripperLeftIface->data->cmdPositionRate = this->jointData[id].cmdSpeed;
- pr2GripperLeftIface->Unlock();
- }
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperRightIface)
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2GripperRightIface->data->controlMode = this->jointData[id].controlMode ;
- pr2GripperRightIface->data->pGain = this->jointData[id].pGain ;
- pr2GripperRightIface->data->iGain = this->jointData[id].iGain ;
- pr2GripperRightIface->data->dGain = this->jointData[id].dGain ;
- pr2GripperRightIface->data->cmdGap = this->jointData[id].cmdGap ;
- pr2GripperRightIface->data->cmdForce = this->jointData[id].cmdEffectorForce;
- pr2GripperRightIface->data->cmdPositionRate = this->jointData[id].cmdSpeed;
- pr2GripperRightIface->Unlock();
- }
- }
- else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2Iface->data->actuators[id].controlMode = this->jointData[id].controlMode ;
- pr2Iface->data->actuators[id].pGain = this->jointData[id].pGain ;
- pr2Iface->data->actuators[id].iGain = this->jointData[id].iGain ;
- pr2Iface->data->actuators[id].dGain = this->jointData[id].dGain ;
- pr2Iface->data->actuators[id].cmdPosition = this->jointData[id].cmdPosition ;
- pr2Iface->data->actuators[id].cmdSpeed = this->jointData[id].cmdSpeed ;
- pr2Iface->data->actuators[id].cmdEffectorForce = this->jointData[id].cmdEffectorForce;
- pr2Iface->Unlock();
- }
+ pr2Iface->Lock(1);
+ pr2Iface->data->actuators[id].cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
+ pr2Iface->data->actuators[id].controlMode = this->jointData[id].controlMode ;
+ pr2Iface->data->actuators[id].pGain = this->jointData[id].pGain ;
+ pr2Iface->data->actuators[id].iGain = this->jointData[id].iGain ;
+ pr2Iface->data->actuators[id].dGain = this->jointData[id].dGain ;
+ pr2Iface->data->actuators[id].cmdPosition = this->jointData[id].cmdPosition ;
+ pr2Iface->data->actuators[id].cmdSpeed = this->jointData[id].cmdSpeed ;
+ pr2Iface->data->actuators[id].cmdEffectorForce = this->jointData[id].cmdEffectorForce;
+ pr2Iface->Unlock();
}
@@ -1176,55 +936,11 @@
// receive data from hardware
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- /* //Don't look at grippers for now
- pr2GripperLeftIface->Lock(1);
- this->jointArray[id].actualPosition = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualGap = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualSpeed = ( pr2GripperLeftIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPositionRate[1]);
- this->jointData[id].actualSpeed = pr2GripperLeftIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
-
- this->jointData[id].actualEffectorForce = pr2GripperLeftIface->data->gripperForce;
- this->jointData[id].actualFingerPosition[0] = pr2GripperLeftIface->data->actualFingerPosition[0];
- this->jointData[id].actualFingerPosition[1] = pr2GripperLeftIface->data->actualFingerPosition[1];
- this->jointData[id].actualFingerPositionRate[0] = pr2GripperLeftIface->data->actualFingerPositionRate[0];
- this->jointData[id].actualFingerPositionRate[1] = pr2GripperLeftIface->data->actualFingerPositionRate[1];
-
- pr2GripperLeftIface->Unlock();
- */
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- /*
- pr2GripperRightIface->Lock(1);
- this->jointData[id].actualPosition = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualGap = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualSpeed = ( pr2GripperRightIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPositionRate[1]);
- this->jointData[id].actualSpeed = pr2GripperRightIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
-
- this->jointData[id].actualEffectorForce = pr2GripperRightIface->data->gripperForce;
- this->jointData[id].actualFingerPosition[0] = pr2GripperRightIface->data->actualFingerPosition[0];
- this->joint...
[truncated message content] |
|
From: <is...@us...> - 2008-08-05 18:18:22
|
Revision: 2557
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2557&view=rev
Author: isucan
Date: 2008-08-05 18:18:28 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
world_3d_map now has support for removing points from pointclouds if they collide with the robot's arms
Modified Paths:
--------------
pkg/trunk/util/collision_space/include/collision_space/util.h
pkg/trunk/world_models/world_3d_map/manifest.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/util/collision_space/include/collision_space/util.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/util.h 2008-08-05 17:39:31 UTC (rev 2556)
+++ pkg/trunk/util/collision_space/include/collision_space/util.h 2008-08-05 18:18:28 UTC (rev 2557)
@@ -57,26 +57,48 @@
m_scale = scale;
}
- bool containsPoint(const libTF::Pose3D &pose, const libTF::Pose3D::Position &p) const
+ void setPose(const libTF::Pose3D &pose)
{
+ m_pose = pose;
+ m_pose.invert();
+ }
+
+ bool containsPoint(const libTF::Pose3D::Position &p) const
+ {
/* bring point in the reference frame described by pose */
libTF::Pose3D::Position pt = p;
- pose.applyToPosition(pt);
+ m_pose.applyToPosition(pt);
/* since the body is centered at origin, scaling the body is equivalent
* to scaling the coordinates of the point */
- pt.x *= m_scale;
- pt.y *= m_scale;
- pt.z *= m_scale;
+ pt.x /= m_scale;
+ pt.y /= m_scale;
+ pt.z /= m_scale;
- return containsPoint(pt);
+ return containsPt(pt);
}
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const = 0;
-
+ bool containsPoint(double x, double y, double z) const
+ {
+ /* bring point in the reference frame described by pose */
+ libTF::Pose3D::Position pt = { x, y, z };
+ m_pose.applyToPosition(pt);
+
+ /* since the body is centered at origin, scaling the body is equivalent
+ * to scaling the coordinates of the point */
+ pt.x /= m_scale;
+ pt.y /= m_scale;
+ pt.z /= m_scale;
+
+ return containsPt(pt);
+ }
+
protected:
+
+ virtual bool containsPt(const libTF::Pose3D::Position &p) const = 0;
- double m_scale;
+ libTF::Pose3D m_pose;
+ double m_scale;
};
@@ -97,13 +119,13 @@
m_radius = radius;
m_radius2 = radius * radius;
}
-
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+
+ protected:
+
+ virtual bool containsPt(const libTF::Pose3D::Position &p) const
{
return p.x * p.x + p.y * p.y + p.z * p.z < m_radius2;
}
-
- protected:
double m_radius2;
double m_radius;
@@ -129,15 +151,15 @@
m_radius2 = radius * radius;
}
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ protected:
+
+ virtual bool containsPt(const libTF::Pose3D::Position &p) const
{
if (fabs(p.z) > m_length2)
return false;
return p.x * p.x + p.y * p.y < m_radius2;
}
- protected:
-
double m_length;
double m_length2;
double m_radius;
@@ -164,14 +186,15 @@
m_width2 = width / 2.0;
m_height = height;
m_height2 = height / 2.0;
- }
+ }
+
+ protected:
- virtual bool containsPoint(const libTF::Pose3D::Position &p) const
+ virtual bool containsPt(const libTF::Pose3D::Position &p) const
{
return fabs(p.x) < m_length2 && fabs(p.y) < m_width2 && fabs(p.z) < m_height2;
}
- protected:
double m_length;
double m_width;
double m_height;
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-05 17:39:31 UTC (rev 2556)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-05 18:18:28 UTC (rev 2557)
@@ -8,5 +8,7 @@
<depend package="xmlparam" />
<depend package="rosTF" />
<depend package="random_utils" />
+<depend package="wg_robot_description_parser" />
+<depend package="robot_motion_planning_models" />
<depend package="collision_space" />
</package>
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 17:39:31 UTC (rev 2556)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 18:18:28 UTC (rev 2557)
@@ -90,7 +90,11 @@
#include <rostools/Log.h>
#include <rosTF/rosTF.h>
#include <random_utils/random_utils.h>
+
+#include <urdf/URDF.h>
+#include <planning_models/kinematic.h>
#include <collision_space/util.h>
+
#include <deque>
#include <cmath>
@@ -107,13 +111,14 @@
advertise<PointCloudFloat32>("world_3d_map");
advertise<rostools::Log>("roserr");
- // NOTE: subscribe to stereo vision point cloud as well... when it becomes available
subscribe("cloud", inputCloud, &World3DMap::pointCloudCallback);
param("world_3d_map/max_publish_frequency", maxPublishFrequency, 0.5);
param("world_3d_map/retain_pointcloud_duration", retainPointcloudDuration, 60.0);
param("world_3d_map/retain_pointcloud_fraction", retainPointcloudFraction, 0.02);
param("world_3d_map/verbosity_level", verbose, 1);
+
+ loadRobotDescriptions();
/* create a thread that does the processing of the input data.
* and one that handles the publishing of the data */
@@ -138,8 +143,52 @@
pthread_join(*processingThread, NULL);
for (unsigned int i = 0 ; i < currentWorld.size() ; ++i)
delete currentWorld[i];
+
+ for (unsigned int i = 0 ; i < robotDescriptions.size() ; ++i)
+ {
+ delete robotDescriptions[i].urdf;
+ delete robotDescriptions[i].kmodel;
+ }
}
+ void loadRobotDescriptions(void)
+ {
+ printf("Loading robot descriptions...\n\n");
+
+ std::string description_files;
+ get_param("robotdesc_list", description_files);
+ std::stringstream sdf(description_files);
+ while (sdf.good())
+ {
+ std::string file;
+ std::string content;
+ sdf >> file;
+ printf("Loading '%s'\n", file.c_str());
+ get_param(file, content);
+ addRobotDescriptionFromData(content.c_str());
+ }
+ printf("\n\n");
+ }
+
+ void addRobotDescriptionFromData(const char *data)
+ {
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadString(data))
+ addRobotDescription(file);
+ else
+ delete file;
+ }
+
+ void addRobotDescription(robot_desc::URDF *file)
+ {
+ planning_models::KinematicModel *kmodel = new planning_models::KinematicModel();
+ kmodel->setVerbose(false);
+ kmodel->build(*file);
+
+ RobotDesc rd = { file, kmodel };
+ robotDescriptions.push_back(rd);
+ }
+
void pointCloudCallback(void)
{
/* The idea is that if processing of previous input data is
@@ -315,9 +364,17 @@
private:
- PointCloudFloat32 inputCloud; //Buffer for recieving cloud
- PointCloudFloat32 toProcess; //Buffer (size 1) for incoming cloud
- std::deque<PointCloudFloat32*> currentWorld;// Pointers to saved clouds
+ struct RobotDesc
+ {
+ robot_desc::URDF *urdf;
+ planning_models::KinematicModel *kmodel;
+ };
+
+ std::vector<RobotDesc> robotDescriptions;
+
+ PointCloudFloat32 inputCloud; //Buffer for recieving cloud
+ PointCloudFloat32 toProcess; //Buffer (size 1) for incoming cloud
+ std::deque<PointCloudFloat32*> currentWorld;// Pointers to saved clouds
rosTFClient tf;
double maxPublishFrequency;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-08-05 22:25:22
|
Revision: 2591
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2591&view=rev
Author: stuglaser
Date: 2008-08-05 22:25:31 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
Changed the array of actuators in HardwareInterface to a vector of pointers.
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
pkg/trunk/mechanism/hardware_interface/manifest.xml
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
Modified: pkg/trunk/controllers/generic_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/generic_controllers/manifest.xml 2008-08-05 22:24:39 UTC (rev 2590)
+++ pkg/trunk/controllers/generic_controllers/manifest.xml 2008-08-05 22:25:31 UTC (rev 2591)
@@ -6,10 +6,11 @@
<license>BSD</license>
<depend package="mechanism_model" />
<depend package="wg_robot_description_parser" />
+ <depend package="stl_utils" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
<cpp cflags='-I${prefix}/include' lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lgeneric_controllers'/>
</export>
-
+
</package>
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-08-05 22:24:39 UTC (rev 2590)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-08-05 22:25:31 UTC (rev 2591)
@@ -132,9 +132,9 @@
// Convert HW Interface commands to MCB-specific buffers
current = current_buffer_;
- for (int i = 0; i < hw_->num_actuators_; ++i)
+ for (int i = 0; i < hw_->actuators_.size(); ++i)
{
- slaves[i]->convertCommand(hw_->actuators_[i].command_, current);
+ slaves[i]->convertCommand(hw_->actuators_[i]->command_, current);
current += slaves[i]->commandSize + slaves[i]->statusSize;
}
@@ -144,9 +144,9 @@
// Convert status back to HW Interface
current = current_buffer_;
last = last_buffer_;
- for (int i = 0; i < hw_->num_actuators_; ++i)
+ for (int i = 0; i < hw_->actuators_.size(); ++i)
{
- slaves[i]->convertState(hw_->actuators_[i].state_, current, last);
+ slaves[i]->convertState(hw_->actuators_[i]->state_, current, last);
current += slaves[i]->commandSize + slaves[i]->statusSize;
last += slaves[i]->commandSize + slaves[i]->statusSize;
}
Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-08-05 22:24:39 UTC (rev 2590)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-08-05 22:25:31 UTC (rev 2591)
@@ -35,6 +35,8 @@
#ifndef HARDWARE_INTERFACE_H
#define HARDWARE_INTERFACE_H
+#include <stl_utils/stl_utils.h>
+
class ActuatorState{
public:
ActuatorState() :
@@ -87,12 +89,14 @@
{
public:
HardwareInterface(int num_actuators)
+ : actuators_(num_actuators, (Actuator*)NULL)
{
- actuators_ = new Actuator[num_actuators];
- num_actuators_ = num_actuators;
}
- Actuator *actuators_;
- int num_actuators_;
+ ~HardwareInterface()
+ {
+ deleteElements(&actuators_);
+ }
+ std::vector<Actuator*> actuators_;
double current_time_;
};
Modified: pkg/trunk/mechanism/hardware_interface/manifest.xml
===================================================================
--- pkg/trunk/mechanism/hardware_interface/manifest.xml 2008-08-05 22:24:39 UTC (rev 2590)
+++ pkg/trunk/mechanism/hardware_interface/manifest.xml 2008-08-05 22:25:31 UTC (rev 2591)
@@ -4,6 +4,7 @@
<author>Eric Berger be...@wi...</author>
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
+<depend package="stl_utils" />
<export>
<cpp cflags="-I${prefix}/include"/>
</export>
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-05 22:24:39 UTC (rev 2590)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-05 22:25:31 UTC (rev 2591)
@@ -77,9 +77,9 @@
{
// Looks up the joint and the actuator used by the transmission.
Robot::IndexMap::iterator joint_it =
- model_.joints_lookup_.find(elt->FirstChildElement("joint")->Attribute("name"));
+ model_.joints_lookup_.find(elt->FirstChildElement("joint")->Attribute("name"));
Robot::IndexMap::iterator actuator_it =
- model_.actuators_lookup_.find(elt->FirstChildElement("actuator")->Attribute("name"));
+ model_.actuators_lookup_.find(elt->FirstChildElement("actuator")->Attribute("name"));
if (joint_it == model_.joints_lookup_.end())
{
// TODO: report: The joint was not declared in the XML file
@@ -91,15 +91,11 @@
continue;
}
- Transmission
- *tr =
- new SimpleTransmission(model_.joints_[joint_it->second], &hw_->actuators_[actuator_it->second], atof(
- elt->FirstChildElement(
- "mechanicalReduction")->Value()), atof(
- elt->FirstChildElement(
- "motorTorqueConstant")->Value()), atof(
- elt->FirstChildElement(
- "pulsesPerRevolution")->Value()));
+ Transmission *tr =
+ new SimpleTransmission(model_.joints_[joint_it->second], hw_->actuators_[actuator_it->second],
+ atof(elt->FirstChildElement("mechanicalReduction")->Value()),
+ atof(elt->FirstChildElement("motorTorqueConstant")->Value()),
+ atof(elt->FirstChildElement("pulsesPerRevolution")->Value()));
model_.transmissions_.push_back(tr);
}
else
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-08-05 22:24:39 UTC (rev 2590)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-08-05 22:25:31 UTC (rev 2591)
@@ -39,6 +39,7 @@
#include <vector>
#include <map>
#include <string>
+#include "stl_utils/stl_utils.h"
#include "mechanism_model/link.h"
#include "mechanism_model/joint.h"
#include "mechanism_model/transmission.h"
@@ -54,12 +55,8 @@
~Robot()
{
- std::vector<Transmission *>::size_type t;
- for (t = 0; t < transmissions_.size(); ++t)
- delete transmissions_[t];
- std::vector<Joint *>::size_type j;
- for (j = 0; j < joints_.size(); ++j)
- delete joints_[j];
+ deleteElements(&transmissions_);
+ deleteElements(&joints_);
}
std::vector<Joint*> joints_;
@@ -82,7 +79,7 @@
IndexMap::iterator it = actuators_lookup_.find(name);
if (it == actuators_lookup_.end())
return NULL;
- return &hw_->actuators_[it->second];
+ return hw_->actuators_[it->second];
}
HardwareInterface *hw_;
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-05 22:24:39 UTC (rev 2590)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-05 22:25:31 UTC (rev 2591)
@@ -4,6 +4,7 @@
<author>Eric Berger be...@wi...</author>
<license>BSD</license>
<depend package='hardware_interface'/>
+<depend package="stl_utils" />
<url>http://pr.willowgarage.com</url>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lmechanism_model -Wl,-rpath,${prefix}/lib"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X