[WIP] ros integration of constraints

examples/boxer_example.py

@@ -217,11 +217,11 @@ def run(self):
             # ---END: Visualizations---
 
     def check_goal_reaching(self, ob):
-        primary_goal = self._goal.primary_goal()
-        goal_dist = np.linalg.norm(ob['robot_0']['joint_state']['position'][:2] - primary_goal.position()) # todo remove hard coded dimension, replace it with fk instead
-        if goal_dist <= primary_goal.epsilon():
-            return True
-        return False
+            primary_goal = self._goal.primary_goal()
+            goal_dist = np.linalg.norm(ob['robot_0']['joint_state']['position'][:2] - primary_goal.position()) # todo remove hard coded dimension, replace it with fk instead
+            if goal_dist <= primary_goal.epsilon():
+                return True
+            return False
 
 def main():
     boxer_example = BoxerMpcExample(sys.argv[1])

examples/config/boxerMpc.yaml

@@ -15,7 +15,7 @@ mpc:
     - 'GoalReaching'
     - 'ConstraintAvoidance'
   weights:
-    w: 1.0
+    wgoal: 1.0
     wvel: [0.0, 0.0, 0.0, 1, 0.01]
     ws: 1e10
     wu: 0.01

robotmpcs/models/inequalities/RadialConstraints.py

@@ -19,6 +19,6 @@ def set_parameters(self, ParamMap, npar):
 
 
     def eval_constraint(self, z, p):
-        ineqs = self.eval_obstacleDistances(z,p,j)
+        ineqs = self.eval_obstacleDistances(z,p)
         return ineqs
 

robotmpcs/planner/mpcPlanner.py

@@ -92,7 +92,7 @@ def reset(self):
         for i in range(self._config.time_horizon):
             self._params[
                 [self._npar * i + val for val in self._paramMap["wgoal"]]
-            ] = self._config.weights["w"]
+            ] = self._config.weights["wgoal"]
             # for j, val in enumerate(self._paramMap["wvel"]):
             #     self._params[[self._npar * i + val]] = self._config.weights["wvel"][j]
             self._params[
@@ -199,8 +199,10 @@ def setGoalReaching(self, goal):
             for j in range(self.m()):
                 if j >= len(goal.primary_goal().position()):
                     position = 0
+                    print('no goal')
                 else:
                     position = goal.primary_goal().position()[j]
+                    print('setting goal')
                 self._params[self._npar * i + self._paramMap["goal"][j]] = position
 
     def setConstraintAvoidance(self):

ros_bridge/README.md

@@ -19,6 +19,7 @@ Install `robot_mpcs` globally (or in the virtual environment if you use ros insi
 ```bash
 pip install -e .
 ```
+ira_laser_tools
 
 Build catkin workspace
 ```bash

ros_bridge/src/robotmpcs_ros/config/boxer_mpc_config.yaml

@@ -6,18 +6,21 @@ mpc:
   time_step: 0.1
   slack: False
   interval: 1
-  initialization: previous_plan
+  initialization: current_state
   constraints:
-    - RadialConstraints
+    - LinearConstraints
     - SelfCollisionAvoidanceConstraints
     - JointLimitConstraints
     - InputLimitConstraints
+  objectives:
+    - 'GoalReaching'
   weights:
-    w: 1.0
-    wvel: [0.0, 0.0, 0.0, 1, 0.01]
+    wgoal: 1.0
+    wvel: [0.0, 0.0, 0.0, 0.0, 0.0]
     ws: 1e10
     wu: 0.01
-    wobst: 0.5
+    wobst: 0.0
+    wconstr: [0.0, 0.0, 0.0, 0.0]
   number_obstacles: 1
   control_mode: "acc"
 robot:
@@ -29,3 +32,5 @@ robot:
   root_link: base_link
   end_link: ee_link
   base_type: diffdrive
+example:
+  debug: False

ros_bridge/src/robotmpcs_ros/launch/lidar_merger.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <node pkg="ira_laser_tools" name="laserscan_multi_merger" type="laserscan_multi_merger">
+    <param name="destination_frame" value="front_laser"/>
+    <param name="scan_destination_topic" value="/scan"/>
+    <param name="laserscan_topics" value ="/front/scan /rear/scan" />
+  </node>
+
+</launch>