minor update for filter and franka-ros

pyrobolearn/simulators/middlewares/robots/franka.py

@@ -30,6 +30,7 @@ except ImportError as e:
           "when resetting the joint states.\n" + str(e))
 
 from pyrobolearn.simulators.middlewares.ros import ROSRobotMiddleware
+from pyrobolearn.utils.filters import MovingAverageFilter
 
 
 __author__ = "Brian Delhaisse"
@@ -107,6 +108,7 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
         # arm_topic = '/position_joint_trajectory_controller/command'
         self.arm_publisher = self.publisher.create_publisher(name='panda_arm_trajectory', topic=arm_topic,
                                                              msg_class=JointTrajectory)
+        self.use_hand = True
         hand_topic = '/panda_hand_controller/command'
         self.hand_publisher = self.publisher.create_publisher(name='panda_hand_trajectory', topic=hand_topic,
                                                               msg_class=JointTrajectory)
@@ -119,10 +121,13 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
 
         # create reset joint state service
         self.reset_joint_service = None
-        if MoveJoints is not None:
+        self.use_real_robot = True
+        if MoveJoints is not None and self.use_real_robot:
             self.reset_joint_service_name = '/arm/move_joint_absolute'
             self.reset_joint_service = rospy.ServiceProxy(self.reset_joint_service_name, MoveJoints)
 
+        self.filter = MovingAverageFilter(alpha=0.3)
+
     def reset_joint_states(self, positions, joint_ids=None, velocities=None):
         """
         Reset the joint states. It is best only to do this at the start, while not running the simulation:
@@ -189,6 +194,8 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
         """
         if self.is_subscribing:
             q_indices = None if joint_ids is None else self.q_indices[joint_ids]
+            if not self.use_hand:
+                q_indices = q_indices[q_indices <= 6]
             return self.subscriber.get_joint_positions(q_indices)
 
     def set_joint_positions(self, positions, joint_ids=None, velocities=None, kps=None, kds=None, forces=None):
@@ -206,10 +213,14 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
         if self.is_publishing:
             q = self.subscriber.get_joint_positions()
             dq = self.subscriber.get_joint_velocities()
+            # dq = self.filter(dq)
             # tau = self.subscriber.get_joint_torques()
 
-            if len(q) > 0:
+            if q is not None and len(q) > 0:
                 q_indices = None if joint_ids is None else self.q_indices[joint_ids]
+                if not self.use_hand:
+                    q_indices = q_indices[q_indices <= 6]
+
                 if q_indices is not None:
                     q[q_indices] = positions
                     if velocities is not None:
@@ -219,9 +230,10 @@ class FrankaROSMiddleware(ROSRobotMiddleware):
                 self.arm_point.velocities = dq[:7]
                 # self.arm_point.effort = tau[:7]
 
-                self.hand_point.positions = q[7:]
-                self.hand_point.velocities = dq[7:]
-                # self.hand_point.effort = tau[7:]
+                if self.use_hand:
+                    self.hand_point.positions = q[7:]
+                    self.hand_point.velocities = dq[7:]
+                    # self.hand_point.effort = tau[7:]
 
                 # set time duration
                 self.arm_point.time_from_start.secs = 0

pyrobolearn/utils/filters.py

@@ -25,15 +25,20 @@ class MovingAverageFilter(Filter):
 
     The moving average filter computes the moving mean given by:
 
-    .. math:: \mu_{N+1} = \frac{N}{N+1} \mu_N + \frac{1}{N+1} x_{N+1}
+    .. math:: \mu_{N+1} = \frac{N}{N+1} \mu_N + \frac{1}{N+1} x_{N+1}`
 
     where :math:`\mu_0 = 1`.
+
+    If an :math:`\alpha` parameter is provided it will compute:
+
+    .. math:: \mu_{N+1} = (1-\alpha) \mu_N + \alpha x_{N+1}
     """
 
-    def __init__(self):
+    def __init__(self, alpha=None):
         """Initialize the moving average filter"""
         self.mu = None
         self.N = 0
+        self.alpha = alpha
 
     def __call__(self, x):
         """
@@ -47,9 +52,11 @@ class MovingAverageFilter(Filter):
         """
         if self.mu is None:
             self.mu, self.N = x, 1
-            return self.mu
-        self.N += 1
-        self.mu = (self.N-1.)/self.N * self.mu + 1./self.N * x
+        elif self.alpha is None:
+            self.N += 1
+            self.mu = (self.N-1.)/self.N * self.mu + 1./self.N * x
+        else:
+            self.mu = (1. - self.alpha) * self.mu + self.alpha * x
         return self.mu
 
 
@@ -229,3 +236,19 @@ class OneEuroFilter(Filter):
         cutoff = self.__mincutoff + self.__beta * np.fabs(edx)
         # filter the given value
         return self.__x(x, timestamp, alpha=self.__alpha(cutoff))
+
+
+if __name__ == '__main__':
+    import matplotlib.pyplot as plt
+
+    # Test the filters
+    one_euro_filter = OneEuroFilter(freq=1, mincutoff=0.5, beta=0.1, dcutoff=1.0)
+    moving_average = MovingAverageFilter(alpha=0.3)
+
+    t = np.linspace(0, 1., 200)
+    x = np.sin(4*np.pi*t) + 0.2 * (np.random.rand(200) - 0.5)
+
+    plt.plot(t, x)
+    plt.plot(t, [moving_average(i) for i in x])
+    plt.plot(t, [one_euro_filter(i, timestamp=i) for i in t])
+    plt.show()