merge

Author: Miguel Cuartin

README.md

@@ -1,39 +1,85 @@
 # YouBot
+[![python](https://img.shields.io/badge/python-v3.7.X-green.svg)](https://www.python.org/)
+[![pip](https://img.shields.io/badge/pip-v10.0.X-yellow.svg)](https://pypi.org/project/pip/)
+[![virtualenv](https://img.shields.io/badge/virtualenv-v15.1.X-red.svg)](https://virtualenv.pypa.io/en/stable/)
 
-ROS packages for KUKA YouBot Robot
+ROS packages for the KUKA YouBot robot.
+
+## Table of Contents
+
+- [Requirements](#requirements)
+- [Installation](#installation)
+- [Quickstart](#quickstart)
+- [Contributing](#contributing)
+- [Further reading / Useful links](#further-reading--useful-links)
+
+## Requirements
+
+Before you begin, ensure you have met the following requirements:
+* You have a Linux machine with Ubuntu Xenial 16.0. LTS
+* You have installed [ROS Kinetic](http://wiki.ros.org/kinetic/Installation)
+* You have installed [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/installing.html)
+* You have installed [Python 2.7](https://www.python.org/downloads/)
 
 **Maintainer:** [Miguel Cuartin Ordaz](https://www.linkedin.com/in/macuartin/)
 
 ## Installation
 
-### Basic Requirements
+Go to your ROS working directory:
 
-  1. Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) (**Base Install** Recommended)
-
-### Repository Installation
-
-Go to your ROS working directory. e.g.
 ```bash
 cd ~/catkin_ws/src
-``` 
+```
+
 Clone the required repositories:
+
 ```bash
-git clone https://github.com/fsuarez6/lenny.git
+git clone https://github.com/macuartin/youbot.git
 ```
-Install any missing dependencies using rosdep:
-```bash
-rosdep update
-rosdep install --from-paths . --ignore-src --rosdistro $ROS_DISTRO
-``` 
-Now compile your ROS workspace. e.g.
+
+Compile your ROS workspace:
+
 ```bash
 cd ~/catkin_ws && catkin_make
-``` 
+```
+
+## Quickstart
 
-### Testing Installation
+Be sure to always source the appropriate ROS setup file, e.g:
 
-Be sure to always source the appropriate ROS setup file:
 ```bash
 source ~/catkin_ws/devel/setup.bash
-``` 
-You might want to add that line to your `~/.bashrc`
+```
+You might want to add the line above to your ~/.bashrc file.
+
+Try the following command:
+
+```bash
+roslaunch youbot_bringup main.launch
+```
+
+## Contributing
+
+To contribute, follow these steps:
+
+1. Fork this repository.
+2. Create a branch: `git checkout -b <branch_name>`.
+3. Make your changes and commit them: `git commit -m '<commit_message>'`
+4. Push to the original branch: `git push origin <project_name>/<location>`
+5. Create the pull request.
+
+Alternatively see the GitHub documentation on [creating a pull request](https://help.github.com/en/github/collaborating-with-issues-and-pull-requests/creating-a-pull-request).
+
+## Further reading / Useful links
+
+* Lorem ipsum dolor sit amet, consectetur adipiscing elit.
+* Lorem ipsum dolor sit amet, consectetur adipiscing elit.
+
+## Contact
+
+If you want to contact me you can reach me at [[email protected]](mailto:[email protected])
+
+## License
+<!--- If you're not sure which open license to use see https://choosealicense.com/--->
+
+This project uses the following license: [<license_name>](<link>).

youbot_bringup/launch/main.launch

@@ -1,5 +1,5 @@
 <launch>
-    <node pkg="youbot_traj_planner" type="traj_planner_publisher.py" name="traj_planner_publisher">
+    <node pkg="youbot_control" type="traj_planner_node.py" name="traj_planner_node">
         <rosparam file="$(find youbot_bringup)/config/youbot_traj_planner.yaml" />
     </node>
 </launch>

youbot_control/CMakeLists.txt

@@ -20,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##

youbot_control/package.xml

@@ -7,7 +7,7 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
-  <maintainer email="[email protected]">macuartin</maintainer>
+  <maintainer email="[email protected]">Miguel Cuartin  </maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->

youbot_control/scripts/traj_planner_node.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import rospy
+from youbot_msgs.msg import Point
+from rospy.numpy_msg import numpy_msg
+from youbot_control.cartesian_traj_generator import cubic_splines_planner
+from youbot_control.trajectory import TrajectoryController
+from numpy import pi
+
+
+if __name__ == '__main__':
+
+    try:
+        checkpoints = rospy.get_param('/traj_planner_node/checkpoints')
+        checkpoints_timing = rospy.get_param('/traj_planner_node/checkpoints_timing')
+        initial_velocity = rospy.get_param('/traj_planner_node/initial_velocity')
+        final_velocity = rospy.get_param('/traj_planner_node/final_velocity')
+        sampling_time = rospy.get_param('/traj_planner_node/sampling_time')
+
+        controller = TrajectoryController(checkpoints,
+                                          checkpoints_timing,
+                                          initial_velocity,
+                                          final_velocity,
+                                          sampling_time,
+                                          'cubic_splines')
+
+        position = controller.get_position_trajectory()
+
+        rospy.init_node('traj_planner_node')
+        pub = rospy.Publisher('traj_planner_topic', numpy_msg(Point), queue_size=10)
+        rate = rospy.Rate(10)
+        msg = Point()
+
+        for point in position:
+            msg.x = point[0]
+            msg.y = point[1]
+            msg.z = point[2]
+
+            pub.publish(msg)
+            rospy.loginfo(msg)
+            rate.sleep()
+
+            if rospy.is_shutdown():
+                break
+
+    except rospy.ROSException as err:
+        rospy.logerr(err)

youbot_control/include/youbot_control/__init__.py renamed to youbot_control/src/youbot_control/__init__.py

@@ -0,0 +1,205 @@
+#! /usr/bin/env python
+from __future__ import division
+import rospy
+import numpy as np
+
+
+class TrajectoryController(object):
+
+    def __init__(self, checkpoints, checkpoints_timing, initial_velocity,
+                 final_velocity, sampling_time, generator):
+
+        self.checkpoints = checkpoints
+        self.checkpoints_timing = checkpoints_timing
+        self.initial_velocity = initial_velocity
+        self.final_velocity = final_velocity
+        self.sampling_time = sampling_time
+        self.generator = generator
+
+    def get_position_trajectory(self):
+        if self.generator == 'cubic_splines':
+            return self.cubic_splines_interpolator('position')
+
+    def get_velocity_trajectory(self):
+        if self.generator == 'cubic_splines':
+            return self.cubic_splines_interpolator('velocity')
+
+    def get_acceleration_trajectory(self):
+        if self.generator == 'cubic_splines':
+            return self.cubic_splines_interpolator('acceleration')
+
+    def get_orientation_trajectory(self):
+        if self.generator == 'cubic_splines':
+            return self.cubic_splines_interpolator('position')
+
+    def __get_a_matrix(self, n, h):
+
+        A = np.zeros(shape=(n, n))
+
+        A[0, 0] = 2 * h[0]
+
+        for i in range(0, n-1):
+            A[i+1, i] = h[i]
+            A[i, i+1] = h[i]
+
+        for i in range(1, n-1):
+            A[i, i] = 2 * (h[i-1] + h[i])
+
+        A[n-1, n-1] = 2 * h[n-2]
+
+        return A
+
+    def __get_F_vector(self, n, h, component):
+
+        F = np.zeros(shape=(n, 1))
+
+        F[0] = (((3/h[0])*(self.checkpoints[1][component]-self.checkpoints[0][component]))
+                - 3*self.initial_velocity[component])
+
+        for i in range(1, n-1):
+            F[i] = (((3/h[i])
+                    * (self.checkpoints[i+1][component]-self.checkpoints[i][component]))
+                    - ((3/h[i-1])
+                    * (self.checkpoints[i][component]-self.checkpoints[i-1][component])))
+
+        F[n-1] = (3*self.final_velocity[component]
+                  - ((3/h[n-2])
+                  * (self.checkpoints[n-1][component]-self.checkpoints[n-2][component])))
+
+        return F
+
+    def __get_b_coefficient(self, A, F):
+        return np.dot(np.linalg.inv(A), F)
+
+    def __get_a_coefficient(self, n, b, h, component):
+        a = np.zeros(shape=(n, 1))
+        for i in range(0, n-1):
+            a[i] = (b[i+1] - b[i]) / (3*h[i])
+        return a
+
+    def __get_c_coefficient(self, n, b, h, component):
+        c = np.zeros(shape=(n, 1))
+        for i in range(0, n-1):
+            c[i] = (((1/h[i])*(self.checkpoints[i+1][component]
+                    - self.checkpoints[i][component]))
+                    - ((h[i]/3)*((2*b[i]) + b[i+1])))
+        return c
+
+    def __get_d_coefficient(self, n, b, h, component):
+        d = np.zeros(shape=(n, 1))
+        for i in range(0, n-1):
+            d[i] = self.checkpoints[i][component]
+        return d
+
+    def cubic_splines_interpolator(self, component):
+
+        '''
+            checkpoints: [[x0,y0,z0], [x1,y1,z1], ... , [xn,yn,zn]].
+
+            checkpoints_timing: [t0, t1, ... , tn ].
+
+            velocity_vector: [[Vx0, Vy0, Vz0], [Vxn, Vyn, Vzn]].
+
+            sampling_time: time in seconds.
+        '''
+
+        # Components
+        x = 0
+        y = 1
+        z = 2
+
+        # Tamano del sistema
+        n = len(self.checkpoints_timing)
+
+        # Vector de diferencias de tiempo.
+        h = []
+        for i in range(0, n-1):
+            h.append(self.checkpoints_timing[i+1] - self.checkpoints_timing[i])
+
+        # Matriz A
+        A = self.__get_a_matrix(n, h)
+
+        # Vector F
+        Fx = self.__get_F_vector(n, h, x)
+        Fy = self.__get_F_vector(n, h, y)
+        Fz = self.__get_F_vector(n, h, z)
+
+        # Calculo de Coeficientes
+        bx = self.__get_b_coefficient(A, Fx)
+        by = self.__get_b_coefficient(A, Fy)
+        bz = self.__get_b_coefficient(A, Fz)
+
+        ax = self.__get_a_coefficient(n, bx, h, x)
+        ay = self.__get_a_coefficient(n, by, h, y)
+        az = self.__get_a_coefficient(n, bz, h, z)
+
+        cx = self.__get_c_coefficient(n, bx, h, x)
+        cy = self.__get_c_coefficient(n, by, h, y)
+        cz = self.__get_c_coefficient(n, bz, h, z)
+
+        dx = self.__get_d_coefficient(n, bx, h, x)
+        dy = self.__get_d_coefficient(n, by, h, y)
+        dz = self.__get_d_coefficient(n, bz, h, z)
+
+        # Calculo de Posicion, Velocidad y Aceleracion.
+        # Tamano de los vectores
+        tv = 0
+        for i in range(0, n-1):
+            tv = tv + len(np.arange(0, h[i] + self.sampling_time, self.sampling_time))
+
+        # Creacion de Vectores
+        T = np.zeros(tv)
+        ori = np.zeros((tv, 3))
+        traj = np.zeros((tv, 3))
+
+        # Variable Auxiliar para acumular la cantidad de tiempos
+        p = 0
+        for i in range(0, n-1):
+            # Tiempo a evaluar en los polinomios
+            # (de 0 al delta de tiempo del tramo)
+            Tk = np.arange(0, h[i] + self.sampling_time, self.sampling_time)
+            # Momento en el cual esta presente el robot en cada posicion
+            tc = (np.arange(self.checkpoints_timing[i],
+                  self.checkpoints_timing[i+1] + self.sampling_time,
+                  self.sampling_time))
+
+            k = len(Tk)
+            for j in range(0, k):
+
+                if component == 'position':
+                    traj[j+p, x] = ((ax[i] * (Tk[j]**3))
+                                    + (bx[i] * (Tk[j]**2))
+                                    + (cx[i]*Tk[j])
+                                    + dx[i])
+
+                    traj[j+p, y] = ((ay[i] * (Tk[j]**3))
+                                    + (by[i] * (Tk[j]**2))
+                                    + (cy[i]*Tk[j])
+                                    + dy[i])
+
+                    traj[j+p, z] = ((az[i] * (Tk[j]**3))
+                                    + (bz[i] * (Tk[j]**2))
+                                    + (cz[i]*Tk[j])
+                                    + dz[i])
+                elif component == 'velocity':
+                    traj[j+p, x] = (3 * ax[i] * (Tk[j]**2)) + (2 * bx[i] * (Tk[j])) + cx[i]
+                    traj[j+p, y] = (3 * ay[i] * (Tk[j]**2)) + (2 * by[i] * (Tk[j])) + cy[i]
+                    traj[j+p, z] = (3 * az[i] * (Tk[j]**2)) + (2 * bz[i] * (Tk[j])) + cz[i]
+                elif component == 'acceleration':
+                    traj[j+p, x] = (6 * ax[i] * (Tk[j])) + (2 * bx[i])
+                    traj[j+p, y] = (6 * ay[i] * (Tk[j])) + (2 * by[i])
+                    traj[j+p, z] = (6 * az[i] * (Tk[j])) + (2 * bz[i])
+
+                # Roll[j+p] = (Tk[j]*(self.checkpoints[i+1][3] - self.checkpoints[i][3])
+                #              + self.checkpoints[i][3])
+
+                # Pitch[j+p] = (Tk[j]*(self.checkpoints[i+1][4] - self.checkpoints[i][4])
+                #               + self.checkpoints[i][4])
+
+                # Yaw[j+p] = (Tk[j]*(self.checkpoints[i+1][5] - self.checkpoints[i][5])
+                #             + self.checkpoints[i][5])
+
+                T[j+p] = tc[j]
+            p = p + k
+
+        return traj