Merge pull request #3 from leggedrobotics/devel

Fix xacro bug and remove terrain and optimization specific classes.

robots/xpp_hyq/launch/biped.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- Upload URDF file to ros parameter server for rviz to find  -->
-  <param name="biped_rviz_urdf_robot_description" command="$(find xacro)/xacro.py '$(find xpp_hyq)/urdf/biped.urdf'"/>
+  <param name="biped_rviz_urdf_robot_description" command="$(find xacro)/xacro.py --inorder '$(find xpp_hyq)/urdf/biped.urdf'"/>
 
   <!-- Converts Cartesian state to joint state and publish TFs to rviz  --> 
   <node name="urdf_visualizer_hyq2" pkg="xpp_hyq" type="urdf_visualizer_hyq2" output="screen"/>

robots/xpp_hyq/launch/hyq.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- Upload URDF file to ros parameter server for rviz to find  -->
-  <param name="hyq_rviz_urdf_robot_description" command="$(find xacro)/xacro.py '$(find xpp_hyq)/urdf/hyq.urdf.xacro'"/>
+  <param name="hyq_rviz_urdf_robot_description" command="$(find xacro)/xacro.py --inorder '$(find xpp_hyq)/urdf/hyq.urdf.xacro'"/>
 
   <!-- Converts Cartesian state to joint state and publish TFs to rviz  --> 
   <node name="urdf_visualizer_hyq4" pkg="xpp_hyq" type="urdf_visualizer_hyq4" output="screen"/>

robots/xpp_hyq/launch/monoped.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- Upload URDF file to ros parameter server for rviz to find  -->
-  <param name="monoped_rviz_urdf_robot_description" command="$(find xacro)/xacro.py '$(find xpp_hyq)/urdf/monoped.urdf'"/>
+  <param name="monoped_rviz_urdf_robot_description" command="$(find xacro)/xacro.py --inorder '$(find xpp_hyq)/urdf/monoped.urdf'"/>
 
   <!-- Converts Cartesian state to joint state and publish TFs to rviz  --> 
   <node name="urdf_visualizer_hyq1" pkg="xpp_hyq" type="urdf_visualizer_hyq1" output="screen"/>

robots/xpp_hyq/src/exe/urdf_visualizer_hyq1.cc

@@ -36,26 +36,26 @@ using namespace xpp;
 
 int main(int argc, char *argv[])
 {
-	::ros::init(argc, argv, "monoped_urdf_visualizer");
+  ::ros::init(argc, argv, "monoped_urdf_visualizer");
 
-	const std::string joint_desired_mono = "xpp/joint_mono_des";
+  const std::string joint_desired_mono = "xpp/joint_mono_des";
 
-	auto ik = std::make_shared<InverseKinematicsHyq1>();
+  auto ik = std::make_shared<InverseKinematicsHyq1>();
   CartesianJointConverter inv_kin_converter(ik,
-                                            xpp_msgs::robot_state_desired,
-                                            joint_desired_mono);
+					    xpp_msgs::robot_state_desired,
+					    joint_desired_mono);
 
   std::vector<UrdfVisualizer::URDFName> joint_names(HyqlegJointCount);
   joint_names.at(HAA) = "haa_joint";
   joint_names.at(HFE) = "hfe_joint";
   joint_names.at(KFE) = "kfe_joint";
 
-	std::string urdf = "monoped_rviz_urdf_robot_description";
-	UrdfVisualizer node_des(urdf, joint_names, "base", "world",
-	                        joint_desired_mono, "monoped");
+  std::string urdf = "monoped_rviz_urdf_robot_description";
+  UrdfVisualizer node_des(urdf, joint_names, "base", "world",
+			  joint_desired_mono, "monoped");
 
-	::ros::spin();
+  ::ros::spin();
 
-	return 1;
+  return 1;
 }
 

robots/xpp_hyq/src/exe/urdf_visualizer_hyq2.cc

@@ -39,14 +39,14 @@ using namespace biped;
 
 int main(int argc, char *argv[])
 {
-	::ros::init(argc, argv, "biped_urdf_visualizer");
+  ::ros::init(argc, argv, "biped_urdf_visualizer");
 
-	const std::string joint_desired_biped = "xpp/joint_biped_des";
+  const std::string joint_desired_biped = "xpp/joint_biped_des";
 
-	auto ik = std::make_shared<InverseKinematicsHyq2>();
+  auto ik = std::make_shared<InverseKinematicsHyq2>();
   CartesianJointConverter inv_kin_converter(ik,
-                                            xpp_msgs::robot_state_desired,
-                                            joint_desired_biped);
+					    xpp_msgs::robot_state_desired,
+					    joint_desired_biped);
 
   int n_ee = ik->GetEECount();
   int n_j  = HyqlegJointCount;
@@ -58,12 +58,12 @@ int main(int argc, char *argv[])
   joint_names.at(n_j*R + HFE) = "R_hfe_joint";
   joint_names.at(n_j*R + KFE) = "R_kfe_joint";
 
-	std::string urdf = "biped_rviz_urdf_robot_description";
-	UrdfVisualizer node(urdf, joint_names, "base", "world",
-	                    joint_desired_biped, "biped");
+  std::string urdf = "biped_rviz_urdf_robot_description";
+  UrdfVisualizer node(urdf, joint_names, "base", "world",
+		      joint_desired_biped, "biped");
 
-	::ros::spin();
+  ::ros::spin();
 
-	return 1;
+  return 1;
 }
 

robots/xpp_hyq/src/exe/urdf_visualizer_hyq4.cc

@@ -44,38 +44,38 @@ using namespace quad;
 
 int main(int argc, char *argv[])
 {
-	::ros::init(argc, argv, "hyq_urdf_visualizer");
+  ::ros::init(argc, argv, "hyq_urdf_visualizer");
 
-	const std::string joint_desired_hyq = "xpp/joint_hyq_des";
+  const std::string joint_desired_hyq = "xpp/joint_hyq_des";
 
-	auto hyq_ik = std::make_shared<InverseKinematicsHyq4>();
-	CartesianJointConverter inv_kin_converter(hyq_ik,
-	                                          xpp_msgs::robot_state_desired,
-	                                          joint_desired_hyq);
+  auto hyq_ik = std::make_shared<InverseKinematicsHyq4>();
+  CartesianJointConverter inv_kin_converter(hyq_ik,
+					    xpp_msgs::robot_state_desired,
+					    joint_desired_hyq);
 
-	// urdf joint names
-	int n_ee = hyq_ik->GetEECount();
-	int n_j  = HyqlegJointCount;
-	std::vector<UrdfVisualizer::URDFName> joint_names(n_ee*n_j);
-	joint_names.at(n_j*LF + HAA) = "lf_haa_joint";
-	joint_names.at(n_j*LF + HFE) = "lf_hfe_joint";
-	joint_names.at(n_j*LF + KFE) = "lf_kfe_joint";
-	joint_names.at(n_j*RF + HAA) = "rf_haa_joint";
-	joint_names.at(n_j*RF + HFE) = "rf_hfe_joint";
-	joint_names.at(n_j*RF + KFE) = "rf_kfe_joint";
-	joint_names.at(n_j*LH + HAA) = "lh_haa_joint";
-	joint_names.at(n_j*LH + HFE) = "lh_hfe_joint";
-	joint_names.at(n_j*LH + KFE) = "lh_kfe_joint";
-	joint_names.at(n_j*RH + HAA) = "rh_haa_joint";
-	joint_names.at(n_j*RH + HFE) = "rh_hfe_joint";
-	joint_names.at(n_j*RH + KFE) = "rh_kfe_joint";
+  // urdf joint names
+  int n_ee = hyq_ik->GetEECount();
+  int n_j  = HyqlegJointCount;
+  std::vector<UrdfVisualizer::URDFName> joint_names(n_ee*n_j);
+  joint_names.at(n_j*LF + HAA) = "lf_haa_joint";
+  joint_names.at(n_j*LF + HFE) = "lf_hfe_joint";
+  joint_names.at(n_j*LF + KFE) = "lf_kfe_joint";
+  joint_names.at(n_j*RF + HAA) = "rf_haa_joint";
+  joint_names.at(n_j*RF + HFE) = "rf_hfe_joint";
+  joint_names.at(n_j*RF + KFE) = "rf_kfe_joint";
+  joint_names.at(n_j*LH + HAA) = "lh_haa_joint";
+  joint_names.at(n_j*LH + HFE) = "lh_hfe_joint";
+  joint_names.at(n_j*LH + KFE) = "lh_kfe_joint";
+  joint_names.at(n_j*RH + HAA) = "rh_haa_joint";
+  joint_names.at(n_j*RH + HFE) = "rh_hfe_joint";
+  joint_names.at(n_j*RH + KFE) = "rh_kfe_joint";
 
-	std::string urdf = "hyq_rviz_urdf_robot_description";
-	UrdfVisualizer hyq_desired(urdf, joint_names, "base", "world",
-	                           joint_desired_hyq, "hyq_des");
+  std::string urdf = "hyq_rviz_urdf_robot_description";
+  UrdfVisualizer hyq_desired(urdf, joint_names, "base", "world",
+			     joint_desired_hyq, "hyq_des");
 
-	::ros::spin();
+  ::ros::spin();
 
-	return 1;
+  return 1;
 }
 

robots/xpp_hyq/urdf/hyq.urdf.xacro

@@ -2,12 +2,11 @@
 
 <robot name="hyq" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <property name="PI" value="3.1415926535897931"/>
-  - <!-- standard distances -->
-- <property name="d_lr" value="0.414"/>
-- <property name="d_fh" value="0.747"/>
-- <property name="d_tbb" value="0.082"/>
-- <property name="d_p1p2" value="0.35"/>
+  <xacro:property name="PI" value="3.1415926535897931"/>
+  <xacro:property name="d_lr" value="0.414"/>
+  <xacro:property name="d_fh" value="0.747"/>
+  <xacro:property name="d_tbb" value="0.082"/>
+  <xacro:property name="d_p1p2" value="0.35"/>
 
 	<!-- HyQ trunk -->
 	<xacro:include filename="$(find xpp_hyq)/urdf/trunk.urdf.xacro"/>
@@ -17,12 +16,8 @@
 
 
 	<!-- Using the macros defined above -->
-
-	<!-- body -->
 	<!-- trunk -->
-	<xacro:hyq_trunk name="base">
-		<origin xyz="-0.443 -0.191 -0.276" rpy="0 0 0"/>
-	</xacro:hyq_trunk>
+	<xacro:hyq_trunk name="base"/>
 
 	<!-- LF leg -->
 	<xacro:hyq_leg

robots/xpp_hyq/urdf/leg.urdf.xacro

@@ -2,19 +2,18 @@
 
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-	<!-- ================================== Leg ================================== -->
-	<!-- The xacro macro xacro:hyq_leg contains: hip assembly, upper and lower leg -->
+	<!-- leg macro definiction -->
 	<xacro:macro name="hyq_leg" params="name parent *origin reflect_hip reflect_upper reflect_front">
 		<!-- Joints -->
 		<!-- Hip assembly joint -->
 		<joint name="${name}_haa_joint" type="revolute">
-			<insert_block name="origin"/>
+			<xacro:insert_block name="origin"/>
 			<parent link="${parent}"/>
 			<child  link="${name}_hipassembly"/>
 			<axis xyz="0 0 1"/>
 			<limit effort="150.0" velocity="12.0" lower="${-7*PI/18}" upper="${PI/6}"/>
-
 		</joint>
+
 		<!-- Upper leg joint -->
 		<joint name="${name}_hfe_joint" type="revolute">
 			<origin xyz="${d_tbb} 0 0" rpy="${reflect_upper*PI/2} 0 0"/>
@@ -24,8 +23,8 @@
 			<limit effort="150.0" velocity="12.0"
           lower="${-(1+reflect_front)*5*PI/36-(1-reflect_front)*7*PI/36}"
           upper="${(1-reflect_front)*5*PI/36+(1+reflect_front)*7*PI/36}"/>
-
 		</joint>
+
 		<!-- Lower leg joint -->
 		<joint name="${name}_kfe_joint" type="revolute">
 			<origin xyz="${d_p1p2} 0 0" rpy="0 0 0"/>
@@ -35,16 +34,15 @@
 			<limit effort="150.0" velocity="12.0"
           lower="${-(1+reflect_front)*7*PI/18+(1-reflect_front)*PI/18}"
           upper="${(1-reflect_front)*7*PI/18-(1+reflect_front)*PI/18}"/>
+		</joint>
 
 		<!-- Foot joint -->
-		</joint>
 		<joint name="${name}_foot_joint" type="fixed">
 			<origin xyz="0.35 0 0" rpy="${PI/2} 0 ${-PI/2}"/>
 			<parent link="${name}_lowerleg"/>
 			<child  link="${name}_foot"/>
 		</joint>
 
-
 		<!-- Links -->
 		<!-- Hip assembly link -->
 		<link name="${name}_hipassembly">
@@ -55,6 +53,7 @@
 				</geometry>
 			</visual>
 		</link>
+
 		<!-- Upper leg link -->
 		<link name="${name}_upperleg">
 			<visual> 
@@ -64,6 +63,7 @@
 				</geometry>
 			</visual>
 		</link>
+
 		<!-- Lower leg link -->
 		<link name="${name}_lowerleg">
 			<visual>
@@ -73,6 +73,7 @@
 				<material name="white"/>
 			</visual>
 		</link>
+
 		<!-- Foot link -->
 		<link name="${name}_foot">
 			<!--<visual>
@@ -81,7 +82,6 @@
 					<sphere radius="0.021"/>
 				</geometry>
 			</visual>-->
-
 		</link>
 
 	</xacro:macro>

robots/xpp_hyq/urdf/trunk.urdf.xacro

@@ -1,18 +1,16 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-
-	<!-- ================================ Trunk ================================ -->
-	<!-- The xacro macro xacro:hyq_base contains: base and trunk -->
+	<!-- trunk macro definition -->
 	<xacro:macro name="hyq_trunk" params="name">
+
 		<!-- Floating-base Joint -->
 		<joint name="floating_base" type="fixed">
 			<origin xyz="0 0 0" rpy="0 0 0"/>
 			<parent link="${name}"/>
 			<child link="trunk"/>
 		</joint>
 
-		<!-- Links -->
 		<!-- Footprint link -->
 		<link name="${name}">
 			<visual>
@@ -21,6 +19,7 @@
 				</geometry>
 			</visual>
 		</link>
+
 		<!-- Trunk link -->
 		<link name="trunk">
 			<visual>

xpp_examples/launch/biped_ex.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- Launch the general part of visualization -->
-  <include file="$(find xpp_vis)/launch/xpp_vis.launch"></include>
+  <include file="$(find xpp_vis)/launch/xpp.launch"></include>
 
   <!-- Upload biped urdf and inverse tf publisher -->
   <include file="$(find xpp_hyq)/launch/biped.launch"></include>

xpp_examples/launch/hyq_ex.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- Launch the general part of visualization -->
-  <include file="$(find xpp_vis)/launch/xpp_vis.launch"></include>
+  <include file="$(find xpp_vis)/launch/xpp.launch"></include>
 
   <!-- Upload biped urdf and inverse tf publisher -->
   <include file="$(find xpp_hyq)/launch/hyq.launch"></include>

xpp_examples/launch/monoped_ex_bag.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- Launch the general part of visualization -->
-  <include file="$(find xpp_vis)/launch/xpp_vis.launch"></include>
+  <include file="$(find xpp_vis)/launch/xpp.launch"></include>
 
   <!-- Upload biped urdf and inverse tf publisher -->
   <include file="$(find xpp_hyq)/launch/monoped.launch"></include>

xpp_examples/launch/monoped_ex_generate.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- Launch the general part of visualization -->
-  <include file="$(find xpp_vis)/launch/xpp_vis.launch"></include>
+  <include file="$(find xpp_vis)/launch/xpp.launch"></include>
 
   <!-- Upload biped urdf and inverse tf publisher -->
   <include file="$(find xpp_hyq)/launch/monoped.launch"></include>

xpp_examples/launch/quadrotor_ex.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- Launch the general part of visualization -->
-  <include file="$(find xpp_vis)/launch/xpp_vis.launch"></include>
+  <include file="$(find xpp_vis)/launch/xpp.launch"></include>
 
   <!-- Upload biped urdf and inverse tf publisher -->
   <include file="$(find xpp_quadrotor)/launch/quadrotor.launch"></include>

xpp_msgs/CMakeLists.txt

@@ -16,11 +16,10 @@ add_message_files(
   FILES
   StateLin3d.msg
   State6d.msg
-  UserCommand.msg
-  OptParameters.msg
   RobotStateCartesianTrajectory.msg
   RobotStateCartesian.msg
   RobotStateJoint.msg
+  RobotParameters.msg
   TerrainInfo.msg
 )