Skip to content
Snippets Groups Projects
Commit 0c2495ea authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[wip/dynamic-graph-bridge-v3] Update for pinocchio>=1.2.0

parent b954084a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
dynamic-graph-bridge-v3/PLIST
+ 0
− 3
View file @ 0c2495ea
......@@ -13,14 +13,11 @@ lib/libros_bridge.so
lib/libros_interpreter.so
lib/libsot_loader.so
lib/pkgconfig/dynamic_graph_bridge.pc
lib/plugin/robot_model.so
lib/plugin/ros_joint_state.so
lib/plugin/ros_publish.so
lib/plugin/ros_subscribe.so
lib/plugin/ros_time.so
${PYTHON_SITELIB}/dynamic_graph/ros/__init__.py
${PYTHON_SITELIB}/dynamic_graph/ros/robot_model/__init__.py
${PYTHON_SITELIB}/dynamic_graph/ros/robot_model/wrap.so
${PYTHON_SITELIB}/dynamic_graph/ros/ros.py
${PYTHON_SITELIB}/dynamic_graph/ros/ros_joint_state/__init__.py
${PYTHON_SITELIB}/dynamic_graph/ros/ros_joint_state/wrap.so
......
dynamic-graph-bridge-v3/depend.mk
+ 0
− 1
View file @ 0c2495ea
......@@ -24,7 +24,6 @@ DEPEND_ABI.dynamic-graph-bridge-v3.hydro?= dynamic-graph-bridge-v3>=3.0.0
SYSTEM_SEARCH.dynamic-graph-bridge-v3=\
'include/dynamic_graph_bridge/config.hh' \
'lib/pkgconfig/dynamic_graph_bridge.pc:/Version/s/[^0-9.]//gp' \
'lib/plugin/robot_model.so' \
'share/dynamic_graph_bridge/robot_pose_publisher' \
'${PYTHON_SITELIB}/dynamic_graph/ros/ros.py'
......
dynamic-graph-bridge-v3/distinfo
+ 2
− 0
View file @ 0c2495ea
......@@ -3,3 +3,5 @@ RMD160 (dynamic_graph_bridge-v3-3.0.0.tar.gz) = ecf35dc5c50c24a8aa2ba9b53fc2d497
Size (dynamic_graph_bridge-v3-3.0.0.tar.gz) = 742833 bytes
SHA1 (patch-aa) = f1a950bd8ccf882c49dc2de373f12db07b5be852
SHA1 (patch-ab) = c30d3ad301b8e7911908e12398f8da00e0ad57bd
SHA1 (patch-ac) = f44cf390bd8e05cfaa73a24429d405c0b3125d27
SHA1 (patch-ad) = d5eb291f46f3aa9a48d121736954d028c938d2d9
dynamic-graph-bridge-v3/patches/patch-ac 0 → 100644
+ 134
− 0
View file @ 0c2495ea
diff -Naurb src/dynamic_graph/ros/__init__.py src/dynamic_graph/ros/__init__.py
--- src/dynamic_graph/ros/__init__.py 2016-04-14 14:43:12.000000000 +0200
+++ src/dynamic_graph/ros/__init__.py 2016-10-05 11:11:51.222830312 +0200
@@ -1,12 +1,34 @@
+from dynamic_graph.sot.dynamics import Dynamic
from ros_publish import RosPublish
from ros_subscribe import RosSubscribe
from ros_joint_state import RosJointState
-from robot_model import RosRobotModel
from ros import Ros
# aliases, for retro compatibility
-ros_import = ros_publish
-ros_export = ros_subscribe
from ros import RosPublish as RosImport
from ros import RosSubscribe as RosExport
+
+
+class RosRobotModel(Dynamic):
+ def __init__(self, name):
+ Dynamic.__init__(self, name)
+ self.namespace = "sot_controller"
+ self.jointsParameterName_ = "jrl_map"
+
+ def setJointsNamesParameter(self):
+ import rospy
+ if self.model is not None:
+ parameter_name = self.namespace + "/" + jointsParameterName_
+ jointsName = []
+ for i in xrange(self.model.njoints):
+ jointsName.append(self.model.names[i])
+ rospy.set_param(parameter_name,jointsName)
+ return
+
+ def setNamespace(self, ns):
+ self.namespace = ns
+ return
+
+ def curConf(self):
+ return self.position.value
diff -Naurb src/robot_model.cpp src/robot_model.cpp
--- src/robot_model.cpp 2016-04-14 14:43:12.000000000 +0200
+++ src/robot_model.cpp 2016-10-05 11:11:51.222830312 +0200
@@ -58,13 +58,6 @@
void RosRobotModel::loadUrdf (const std::string& filename)
{
- // jrl::dynamics::urdf::Parser parser;
-
- //TODO: Specific rep name. link them to the operational frames in pinocchio
- // std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
- // for (;it!=specialJoints_.end();++it) {
- // parser.specifyREPName(it->first, it->second);
- // }
rosInit (false);
m_model = se3::urdf::buildModel(filename);
this->m_urdfPath = filename;
@@ -89,13 +82,6 @@
void RosRobotModel::loadFromParameterServer()
{
- //jrl::dynamics::urdf::Parser parser;
-
- //TODO: Specific rep name. link them to the operational frames in pinocchio
- // std::map<std::string, std::string>::const_iterator it = specialJoints_.begin();
- // for (;it!=specialJoints_.end();++it) {
- // parser.specifyREPName(it->first, it->second);
- // }
rosInit (false);
std::string robotDescription;
ros::param::param<std::string> ("/robot_description", robotDescription, "");
@@ -119,7 +105,8 @@
XmlRpc::XmlRpcValue JointsNamesByRank_;
JointsNamesByRank_.setSize(m_model.names.size());
- std::vector<std::string>::const_iterator it = m_model.names.begin()+2; //first joint is universe, second is freeflyer
+ //first joint is universe, second is freeflyer
+ std::vector<std::string>::const_iterator it = m_model.names.begin()+2;
for (int i=0;it!=m_model.names.end();++it, ++i) JointsNamesByRank_[i]= (*it);
nh.setParam(jointsParameterName_, JointsNamesByRank_);
}
diff -Naurb src/ros_joint_state.cpp src/ros_joint_state.cpp
--- src/ros_joint_state.cpp 2016-10-05 11:33:23.580811195 +0200
+++ src/ros_joint_state.cpp 2016-10-05 11:11:51.210829997 +0200
@@ -36,23 +36,23 @@
}
namespace {
- void buildJointNames (sensor_msgs::JointState& jointState, se3::Model& robot_model) {
+ void buildJointNames (sensor_msgs::JointState& jointState, se3::Model* robot_model) {
int cnt = 0;
- for (int i=1;i<robot_model.nbody;i++) {
+ for (int i=1;i<robot_model->njoints;i++) {
// Ignore anchors.
- if (se3::nv(robot_model.joints[i]) != 0) {
+ if (se3::nv(robot_model->joints[i]) != 0) {
// If we only have one dof, the dof name is the joint name.
- if (se3::nv(robot_model.joints[i]) == 1) {
- jointState.name[cnt] = robot_model.names[i];
+ if (se3::nv(robot_model->joints[i]) == 1) {
+ jointState.name[cnt] = robot_model->names[i];
cnt++;
}
else {
// ...otherwise, the dof name is the joint name on which
// the dof id is appended.
- int joint_dof = se3::nv(robot_model.joints[i]);
+ int joint_dof = se3::nv(robot_model->joints[i]);
for(int j = 0; j<joint_dof; j++) {
boost::format fmt("%1%_%2%");
- fmt % robot_model.names[i];
+ fmt % robot_model->names[i];
fmt % j;
jointState.name[cnt + j] = fmt.str();
}
@@ -85,14 +85,14 @@
return Value ();
}
- se3::Model& robot_model = dynamic->m_model;
- if (robot_model.nbody == 1)
+ se3::Model* robot_model = dynamic->m_model;
+ if (robot_model->njoints == 1)
{
std::cerr << "no robot in the dynamic entity" << std::endl;
return Value ();
}
- entity.jointState ().name.resize (robot_model.nv);
+ entity.jointState ().name.resize (robot_model->nv);
buildJointNames (entity.jointState (), robot_model);
return Value ();
}
dynamic-graph-bridge-v3/patches/patch-ad 0 → 100644
+ 11
− 0
View file @ 0c2495ea
--- CMakeLists.txt 2016-10-05 11:28:37.653323847 +0200
+++ CMakeLists.txt 2016-10-05 11:11:51.790845238 +0200
@@ -132,7 +132,7 @@
target_link_libraries(ros_joint_state "${DYNAMIC_GRAPH_PLUGINDIR}/dynamic.so")
-compile_plugin(robot_model)
+#compile_plugin(robot_model)
# ros_interperter library.
add_library(ros_interpreter src/ros_interpreter.cpp)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment