revert to stable version with compatible kdl version; moved beta to d…

…ifferent branch
justagist · May 18, 2020 · fcf90ab · fcf90ab
1 parent 40fc3f4
commit fcf90ab
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 13 deletions.
diff --git a/README.md b/README.md
@@ -16,10 +16,10 @@ A **Gazebo simulator** for the Franka Emika Panda robot with ROS interface, prov
 - Supports MoveIt planning and control for Franka Panda Emika robot and arm and Franka Gripper.
 
 ### Continuous Integration Builds
-  
-  ROS Kinetic (master branch) | ROS Melodic (melodic-devel branch)
------------ | -----------
-[![Build Status](https://travis-ci.org/justagist/panda_simulator.svg?branch=master)](https://travis-ci.org/justagist/panda_simulator) | [![Build Status](https://travis-ci.org/justagist/panda_simulator.svg?branch=melodic-devel)](https://travis-ci.org/justagist/panda_simulator)
+
+ROS Melodic (melodic-devel branch): [![Build Status](https://travis-ci.org/justagist/panda_simulator.svg?branch=melodic-devel)](https://travis-ci.org/justagist/panda_simulator)
+
+ROS Kinetic (master branch) [*NOT MAINTAINED, OUTDATED*]: [![Build Status](https://travis-ci.org/justagist/panda_simulator.svg?branch=master)](https://travis-ci.org/justagist/panda_simulator)
 
   ![vid](_extra/panda_simulator.gif)
  Watch video [here](https://www.youtube.com/watch?v=NdSbXC0r7tU).
@@ -48,7 +48,9 @@ Update dependency packages:
     wstool init
     wstool merge panda_simulator/dependencies.rosinstall
     wstool up
-    cd orocos_kinematics_dynamics && git submodule update --recursive --init 
+
+    # use old ros-compatible version of kdl
+    cd orocos_kinematics_dynamics && git checkout b35c424e77ebc5b7e6f1c5e5c34f8a4666fbf5bc && cd ..
     cd ../.. && rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO
 
 Once the dependencies are met, the package can be installed using catkin_make:

diff --git a/panda_gazebo/src/kdl_methods.cpp b/panda_gazebo/src/kdl_methods.cpp
@@ -58,7 +58,7 @@ int KDLMethods::PosFKJntToCart(const KDL::JntArray& q_in, KDL::Frame& p_out, int
     else{
         int j=0;
         for(unsigned int i=0;i<segmentNr;i++){
-            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::Fixed){
+            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
                 p_out = p_out*chain_.getSegment(i).pose(q_in(j));
                 j++;
             }else{
@@ -78,7 +78,7 @@ int KDLMethods::RNECartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot,
         //Sweep from root to leaf
         for(unsigned int i=0;i<ns;i++){
             double q_,qdot_,qdotdot_;
-            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::Fixed){
+            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
                 q_=q(j);
                 qdot_=q_dot(j);
                 qdotdot_=q_dotdot(j);
@@ -114,7 +114,7 @@ int KDLMethods::RNECartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot,
         //Sweep from leaf to root
         j=nj-1;
         for(int i=ns-1;i>=0;i--){
-            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::Fixed){
+            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
                 torques(j)=dot(S[i],f[i]);
                 torques(j)+=chain_.getSegment(i).getJoint().getInertia()*q_dotdot(j);  // add torque from joint inertia
                 --j;
@@ -152,7 +152,7 @@ int KDLMethods::JacobianJntToJac(const KDL::JntArray& q_in, KDL::Jacobian& jac,
         KDL::Frame total;
         for (unsigned int i=0;i<segmentNr;i++) {
             //Calculate new Frame_base_ee
-            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::Fixed){
+            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
                 //pose of the new end-point expressed in the base
                 total = T_tmp*chain_.getSegment(i).pose(q_in(j));
                 //changing base of new segment's twist to base frame if it is not locked
@@ -168,7 +168,7 @@ int KDLMethods::JacobianJntToJac(const KDL::JntArray& q_in, KDL::Jacobian& jac,
             KDL::changeRefPoint(jac,total.p-T_tmp.p,jac);
 
             //Only increase jointnr if the segment has a joint
-            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::Fixed){
+            if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None){
                 //Only put the twist inside if it is not locked
                 if(!locked_joints_[j])
                     jac.setColumn(k++,t_tmp);
@@ -196,7 +196,7 @@ int KDLMethods::JacobianJntToJac(const KDL::JntArray& q_in, KDL::Jacobian& jac,
     {
       //Collect RigidBodyInertia
           Ic[i]=chain_.getSegment(i).getInertia();
-          if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::Fixed)
+          if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None)
       {
           q_=q(k);
           k++;
@@ -221,7 +221,7 @@ int KDLMethods::JacobianJntToJac(const KDL::JntArray& q_in, KDL::Jacobian& jac,
         } 
 
       F=Ic[i]*S[i];
-      if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::Fixed)
+      if(chain_.getSegment(i).getJoint().getType()!=KDL::Joint::None)
       {
           H(k,k)=dot(S[i],F);
           H(k,k)+=chain_.getSegment(i).getJoint().getInertia();  // add joint inertia
@@ -233,7 +233,7 @@ int KDLMethods::JacobianJntToJac(const KDL::JntArray& q_in, KDL::Jacobian& jac,
           F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
           l--; //go down a segment
 
-          if(chain_.getSegment(l).getJoint().getType()!=KDL::Joint::Fixed) //if the joint connected to segment is not a fixed joint
+          if(chain_.getSegment(l).getJoint().getType()!=KDL::Joint::None) //if the joint connected to segment is not a fixed joint
           {    
             j--;
             H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment 

diff --git a/panda_simulator_examples/scripts/move_robot.py b/panda_simulator_examples/scripts/move_robot.py
@@ -121,6 +121,7 @@ def send_to_neutral():
 
         pubmsg.position = vals # JointCommand msg has other fields (velocities, efforts) for
                                # when controlling in other control mode
+        # pubmsg.efforts = [0.,0.,0.,0.,0.,0.,0.]
         pubmsg.mode = pubmsg.POSITION_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE)
 
         pub.publish(pubmsg)