i forgot what i did last time

Arduino/4_vl53l7cx_clean_with_STlibrary_Shift_register_scalebal_to_18/4_vl53l7cx_clean_with_STlibrary_Shift_register_scalebal_to_18.ino

@@ -217,8 +217,8 @@ void processSensorData(VL53L7CX &sensor, VL53L7CX_ResultsData &results, const Js
     JsonArray sensorData = doc[sensorKey].to<JsonArray>(); 
 
     // Process the results and add them to the JSON array
-    for (int y = imageWidth * (imageWidth - 1); y >= 0; y -= imageWidth) {
-      for (int x = 0; x <= imageWidth - 1; x++) {
+    for (int  y = 0; y <= imageWidth * (imageWidth - 1); y += imageWidth) {
+      for (int x = imageWidth - 1; x >= 0;  x--) {
         sensorData.add(results.distance_mm[VL53L7CX_NB_TARGET_PER_ZONE * (x + y)]);
       }
     }

Arduino/libraries/DCMotor/README.md

+# Arduino DCMotor Library v0.1.0
+
+![Arduino Logo](./ArduinoCommunityLogo.png)
+
+## Overview
+
+The Arduino DCMotor library provides a simple and modular way to manage a DC motor connected to an Arduino board using the **L293D** motor driver.
+
+## Installation
+
+1. **Download the Library:**
+   - Click on the "Code" button in the GitHub repository.
+   - Select "Download ZIP" to download the library as a ZIP file.
+
+2. **Extract the ZIP File:**
+   - Extract the contents of the ZIP file to your Arduino libraries directory.
+
+3. **Restart the Arduino IDE:**
+   - Restart the Arduino IDE to ensure that the library is recognized.
+
+## Usage
+
+1. **Include the Library:**
+   ```cpp
+   #include <DCMotor.h>
+   ```
+
+2. **Instantiate the VibrationMotor object:**
+   ```cpp
+   // Specify the pins of the L293D motor driver: (A, B, S)
+   // A --> direction A
+   // B --> direction B
+   // S --> speed pin (PWM)
+   DCMotor motor(2, 4, 9); 
+   ```
+
+## Features
+
+- Constructor
+   ```cpp
+      // Initialize the VibrationMotor with the specified Arduino pin.
+      VibrationMotor(uint8_t motorPin);
+   ```
+
+- Turn the Motor On
+   ```cpp
+      // Turn on the motor with a given speed. 
+      // The speed in in range [-255, 255] where the sign determines the direction.
+      void on(int speed);
+   ```
+
+- Turn the Motor On for a given time
+   ```cpp
+      // Turn on the motor with a given speed and for a given time in millisec. After, the motor turns off.
+      void on(int speed, int millisec);
+   ```
+
+- Turn the Motor Off
+   ```cpp
+      void off();
+   ```
\ No newline at end of file

Arduino/libraries/DCMotor/examples/01.pulse/01.onoff.ino

+#include <DCMotor.h>
+
+/**
+ * DCMotor on/off example alternating on directions.
+*/
+
+
+
+DCMotor motor = DCMotor(2, 4, 9);
+
+void setup() {
+
+}
+
+void loop() {
+
+  int speed = 150;
+  int t = 1000;
+
+  motor.on(speed, t);
+  
+  delay(2000);
+  
+  motor.on(-speed, t);
+
+  delay(2000)
+
+}
\ No newline at end of file

Arduino/libraries/DCMotor/library.properties

+name=DCMotor
+version=0.1.0
+author=Graziano Blasilli
+maintainer=Graziano Blasilli <graziano.blasilli@uniroma1.it>
+sentence=Arduino DCMotor library with the L293D motor driver.
+paragraph=The Arduino DCMotor library provides a simple and modular way to manage a DC motor connected to an Arduino board using the L293D motor driver.
+category=Device Control
+url=https://github.com/ArduinoSapienza/DCMotor
+architectures=*
\ No newline at end of file

Arduino/libraries/DCMotor/src/DCMotor.cpp

+/*
+  DCMotor.h - implementation.
+  Copyright (c) 2023 Graziano Blasilli.
+*/
+
+#include "DCMotor.h"
+
+// Constructor.
+DCMotor::DCMotor(uint8_t pinA, uint8_t pinB, uint8_t pinS) {
+  this->pinA = pinA;
+  this->pinB = pinB;
+  this->pinS = pinS;
+
+  pinMode(this->pinA, OUTPUT);
+  pinMode(this->pinB, OUTPUT);
+  pinMode(this->pinS, OUTPUT);
+}
+
+// Turn on the motor.
+void DCMotor::on(int speed) {
+  this->off();
+
+  // Ensure speed is within valid range (0-255)
+  speed = constrain(speed, -255, 255);
+      
+  // Set the motor direction based on the sign of the speed
+  digitalWrite(this->pinA, speed >= 0 ? HIGH : LOW);
+  digitalWrite(this->pinB, speed >= 0 ? LOW : HIGH);
+
+  // Set the motor speed using PWM
+  analogWrite(this->pinS, abs(speed));
+}
+
+// Turn on the motor for a specified time.
+void DCMotor::on(int speed, int millisec) {
+  this->on(speed);
+  delay(millisec);
+  this->off();
+}
+
+// Turn off the motor.
+void DCMotor::off() {
+  digitalWrite(this->pinA, LOW);
+  digitalWrite(this->pinB, LOW);
+  analogWrite(this->pinS, 0);
+}
\ No newline at end of file

Arduino/libraries/DCMotor/src/DCMotor.h

+/*
+  DCMotor.h - definition.
+  Copyright (c) 2023 Graziano Blasilli.
+*/
+
+
+#ifndef DCMotor_H
+#define DCMotor_H
+
+#include <Arduino.h>
+
+class DCMotor {
+  private:
+    uint8_t pinA;
+    uint8_t pinB;
+    uint8_t pinS;
+  
+  public:
+    // Constructor
+    DCMotor(uint8_t pinA, uint8_t pinB, uint8_t pinS);
+
+   // Turn on the motor.
+    void on(int speed);
+
+    // Turn on the motor for a specified time.
+    void on(int speed, int millisec);
+
+    // Turn off the motor.
+    void off();
+};
+
+#endif
\ No newline at end of file

Arduino/libraries/L293D/CHANGELOG.md

+# Changelog
+
+## [v1.0.3](https://github.com/bjoernboeckle/L293D/tree/v1.0.3)
+- Added "GetCurrentMotorSpeed" function to read current set speed
+- Added "SetStopPWMValue" function to use a custom PWM value for a stopped motor
+
+
+## [v1.0.2](https://github.com/bjoernboeckle/L293D/tree/v1.0.2)
+- Added example for ESP32
+- Fixes for ESP32
+
+
+## [v1.0.1](https://github.com/bjoernboeckle/L293D/tree/v1.0.1)
+- Fixed resolution for ESP32
+- Improved examples
+
+## [v1.0.0](https://github.com/bjoernboeckle/L293D/tree/v1.0.0)
+- Initial release
\ No newline at end of file

Arduino/libraries/L293D/LICENSE

+MIT License
+
+Copyright (c) 2024 bjoernboeckle
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

Arduino/libraries/L293D/README.md

+# L293D
+
+This repository contains a library to control a motor connected to an H-Bridge (i.e. L293D), that can be used with Arduino and ESP32. 
+
+The ESP32 version can also configure the PWM frequency and resolution, also a channel is required.
+
+
+To stop the motor a speed of zero can be used, by default the library sets the pwm output to 100% and disables the motor signals.
+
+If required, this can be changed by using the "SetStopPWMValue" function and setting the stop pwm to 0.
+
+
+
+# Quick start
+
+## Simple example ESP32
+
+
+```c++
+#include <Arduino.h>
+#include <L293D.h>
+
+#define MOTOR_A      1   // motor pin a
+#define MOTOR_B      2   // motor pin b
+#define MOTOR_ENABLE 3   // Enable (also PWM pin)
+
+#define PWM_MOTOR_FREQUENCY   200
+#define PWM_MOTOR_RESOLUTION    8
+
+// Create motor object using given pins
+L293D motor(MOTOR_A, MOTOR_B, MOTOR_ENABLE);
+
+void setup() {  
+    // begin --> true false, enables disables PWM, use given frequency and resolution
+    motor.begin(true, PWM_MOTOR_FREQUENCY, PWM_MOTOR_RESOLUTION);
+    // Speed -100%...0..100%
+    motor.SetMotorSpeed(100);
+}
+
+void loop() {
+}
+```
+
+
+## Simple example Arduino
+
+
+```c++
+#include <Arduino.h>
+#include <L293D.h>
+
+#define MOTOR_A      1   // motor pin a
+#define MOTOR_B      2   // motor pin b
+#define MOTOR_ENABLE 3   // Enable (also PWM pin)
+
+// Create motor object using given pins
+L293D motor(MOTOR_A, MOTOR_B, MOTOR_ENABLE);
+
+void setup() {  
+    // begin --> true false, enables disables PWM
+    motor.begin(true);
+    // Speed -100%...0..100%
+    motor.SetMotorSpeed(100);
+}
+
+void loop() {
+}
+```
+
+# Usage
+
+Create a motor, either only one direction or two directions including enable(pwm).
+Using Pin1 and Pin2 to connect to the motor, and pin3 for pwm and using pwm channel 0 (not supported for Arduino).
+In case Pin is -1 (not set)
+
+```c++
+L293D motor(1, 2, 3, 0);
+```
+
+Initialize pins etc.
+Optional parameter to set PWM mode and also PWM frequency.
+
+```c++
+motor.begin(true, 1000); 
+```
+
+
+
+## Methods
+
+```c++
+//ESP32
+    L293D(int Motor1, int Motor2 = -1, int enablePin = -1, int pwmChannel = 0);
+    bool begin(bool usePwm = false, int frequency = 1000, int resolution = 8);
+
+// Arduino
+    L293D::L293D(int MotorA, int MotorB, int enablePin);
+    bool begin(bool usePwm = false);
+
+bool Stop();
+bool SetMotorSpeed(double speedPercent);
+```
\ No newline at end of file

Arduino/libraries/L293D/examples/simple/simple.ino

+#include <Arduino.h>
+#include <L293D.h>
+
+#define MOTOR_A      1   // motor pin a
+#define MOTOR_B      2   // motor pin b
+#define MOTOR_ENABLE 3   // Enable (also PWM pin)
+
+// Create motor object using given pins
+L293D motor(MOTOR_A, MOTOR_B, MOTOR_ENABLE);
+
+void setup() {  
+    // begin --> true false, enables disables PWM
+    motor.begin(true);
+    // Speed -100%...0..100%
+    motor.SetMotorSpeed(100);
+}
+
+void loop() {
+}
\ No newline at end of file

Arduino/libraries/L293D/examples/simple_esp32/simple_esp32.ino

+#include <Arduino.h>
+#include <L293D.h>
+
+#define MOTOR_A      1   // motor pin a
+#define MOTOR_B      2   // motor pin b
+#define MOTOR_ENABLE 3   // Enable (also PWM pin)
+
+#define PWM_MOTOR_FREQUENCY   200
+#define PWM_MOTOR_RESOLUTION    8
+
+// Create motor object using given pins
+L293D motor(MOTOR_A, MOTOR_B, MOTOR_ENABLE);
+
+void setup() {  
+    // begin --> true false, enables disables PWM, use given frequency and resolution
+    motor.begin(true, PWM_MOTOR_FREQUENCY, PWM_MOTOR_RESOLUTION);
+    // Speed -100%...0..100%
+    motor.SetMotorSpeed(100);
+}
+
+void loop() {
+}
\ No newline at end of file

Arduino/libraries/L293D/keywords.txt

+==================================
+CLASS
+==================================
+L293D	KEYWORD1
+
+==================================
+FUNCTIONS
+==================================
+begin	KEYWORD2
+Stop	KEYWORD2
+SetMotorSpeed	KEYWORD2
+FreeRun	KEYWORD2
+GetCurrentMotorSpeed	KEYWORD2
+SetStopPWMValue	KEYWORD2
+
+==================================
+CONSTANTS
+==================================
+L293D_H	LITERAL1
+INVALID_PIN	LITERAL1
+
+==================================
+DATA TYPES
+==================================

Arduino/libraries/L293D/library.json

+{
+    "name": "L293D",
+    "keywords": "L293D, esp32, arduino, motor, controller, AC motor",
+    "description": "Control a motor with a L293D",
+    "repository":
+    {
+      "type": "git",
+      "url": "https://github.com/bjoernboeckle/L293D.git"
+    },
+    "license": "MIT",
+    "version": "1.0.3",
+    "frameworks": "*",
+    "platforms": "*"
+  }
+  
\ No newline at end of file

Arduino/libraries/L293D/library.properties

+name=L293D
+version=1.0.3
+author=bjoernboeckle
+maintainer=bjoernboeckle
+sentence=Allows an Arduino board to use control a motor using a L293D.
+paragraph=Allows an Arduino board or ESP32 to use control a motor using a L293D.
+category=Device Control
+url=https://github.com/bjoernboeckle/L293D.git
+architectures=*
\ No newline at end of file

Arduino/libraries/L293D/src/L293D.cpp

+#include <Arduino.h>
+#include "L293D.h"
+
+#ifdef ESP32
+  L293D::L293D(int MotorA, int MotorB, int enablePin, int pwmChannel)
+#else
+  L293D::L293D(int MotorA, int MotorB, int enablePin)
+#endif
+{
+  _MotorA = MotorA;
+  _MotorB = MotorB;
+  _enablePin = enablePin;
+  _initialized = false;
+  _currentSpeed = 0;
+  _usePwm = false;
+  _resolutionFactor = 255;
+  _stopPWMValue = 100;
+
+#ifdef ESP32
+  _frequency = 1000;
+  _resolution = 8;
+  _pwmChannel = pwmChannel;
+  _resolutionFactor = (1<<_resolution) - 1;
+#endif
+
+}
+
+#ifdef ESP32
+  bool L293D::begin(bool usePwm, int frequency, int resolution)
+#else
+  bool L293D::begin(bool usePwm)
+#endif
+{
+  if ((_MotorA == INVALID_PIN && _MotorB == INVALID_PIN) || (usePwm && _enablePin == INVALID_PIN))
+    return false;
+
+  _usePwm = usePwm;
+  if (usePwm)
+  {
+#ifdef ESP32
+    SetupPwm(frequency, resolution);
+#endif
+  }
+  else
+    pinMode(_enablePin, OUTPUT);
+
+  if (_MotorA != INVALID_PIN)
+    pinMode(_MotorA, OUTPUT);
+  if (_MotorB != INVALID_PIN)
+    pinMode(_MotorB, OUTPUT);
+
+  _initialized = true;
+  return true;
+}
+
+#ifdef ESP32
+bool L293D::SetupPwm(int frequency, int resolution)
+{
+  if (_enablePin == INVALID_PIN || !_usePwm)
+    return false;
+
+  _frequency = frequency;
+  _resolution = resolution;
+  _resolutionFactor = (1<<_resolution) - 1;  
+
+  if (_initialized)
+    ledcDetachPin(_enablePin);
+  ledcSetup(_pwmChannel, _frequency, _resolution);
+  ledcAttachPin(_enablePin, _pwmChannel);
+
+  return true;
+}
+#endif
+
+bool L293D::FreeRun()
+{
+  if (!_initialized)
+    return false;
+
+  if (_usePwm)
+  {
+#ifdef ESP32
+  ledcWrite(_pwmChannel, 0);
+#else
+  analogWrite(_enablePin, 0);
+#endif
+  }
+  else
+    digitalWrite(_enablePin, LOW);
+
+
+  if (_MotorA != INVALID_PIN)
+    digitalWrite(_MotorA, HIGH);
+  if (_MotorB != INVALID_PIN)
+    digitalWrite(_MotorB, HIGH);
+  
+  return true;
+}
+
+bool L293D::SetMotorSpeed(double speedPercent)
+{
+  if (!_initialized)
+    return false;
+
+  if (_MotorA != INVALID_PIN)
+    digitalWrite(_MotorA, speedPercent > 0 ? HIGH : LOW);
+  if (_MotorB != INVALID_PIN)
+    digitalWrite(_MotorB, speedPercent < 0 ? HIGH : LOW);
+
+  if (_usePwm)
+  {
+    double internalSpeed;
+    
+    // in case 0, stop motor, use LOW for both Motor_A and Motor_B and High for Enable --> Fast motor stop (can be changed by SetStopPWMValue)
+    //EN  1A  2A  FUNCTION(1)
+    //H    L   L  Fast motor stop
+    //H    H   H  Fast motor stop
+    //L    X   X  Free-running motor stop    
+    if (speedPercent == 0)
+    {
+      // deafult max PWM --> HIGH for fast motor stop, or use changed _stopPWMValue
+      internalSpeed = _stopPWMValue * _resolutionFactor / 100.0; 
+    }
+    else
+    {
+      // scale 0..100% to 0..resolution PWM i.e 8 Bit ==>  max 255 PWM
+      internalSpeed = speedPercent * _resolutionFactor / 100.0;
+    }
+
+
+#ifdef ESP32
+    ledcWrite(_pwmChannel, internalSpeed >= 0 ? internalSpeed : -internalSpeed);
+#else
+    analogWrite(_enablePin, internalSpeed >= 0 ? internalSpeed : -internalSpeed);
+#endif
+  }
+  else
+  {
+    digitalWrite(_enablePin, HIGH);
+  }
+
+  _currentSpeed = speedPercent;
+
+  return true;
+}

Arduino/libraries/L293D/src/L293D.h

+#ifndef L293D_H
+#define L293D_H
+
+#define INVALID_PIN     -1
+
+class L293D 
+{
+
+public:    
+    #ifdef ESP32
+        L293D(int MotorA, int MotorB, int enablePin = INVALID_PIN, int pwmChannel = 0);
+    #else
+        L293D(int MotorA, int MotorB, int enablePin = INVALID_PIN);
+    #endif
+
+    #ifdef ESP32
+        bool begin(bool usePwm = true, int frequency = 1000, int resolution = 8);
+    #else
+        bool begin(bool usePwm = false);
+    #endif
+
+    bool FreeRun();
+    bool Stop() { return SetMotorSpeed(0); }
+    bool SetMotorSpeed(double speedPercent);
+    double GetCurrentMotorSpeed() {return _currentSpeed;}
+
+    // set pwm output in percentage to stop motor (default 100%, fast motor stop, could be set to 0, to disable pwm output for stopping the motor)
+    void SetStopPWMValue(double pwm = 100) { _stopPWMValue = pwm; }
+    
+private:
+
+    #ifdef ESP32
+        bool SetupPwm(int frequency = 1000, int resolution = 8); 
+        int _frequency;
+        int _resolution;
+        int _pwmChannel;
+    #endif
+
+    int _resolutionFactor;
+    double _currentSpeed;
+    bool _usePwm;
+    int _enablePin;
+    int _MotorA;
+    int _MotorB;
+    bool _initialized;
+
+    double _stopPWMValue;
+};
+
+
+#endif
\ No newline at end of file

workspaces/sensor_ws/src/ser_test/ser_test/pcl_rob_node.py → workspaces/COLCON_WS/build/ser_test/build/lib/ser_test/pcl_rob_node.py

 import rclpy
+import pyglet
 import rclpy.duration
 from rclpy.node import Node
 from sensor_msgs.msg import PointCloud2
@@ -26,27 +27,84 @@ class PointCloudProcessor(Node):
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
-        # Load the STL file
-        self.mesh_base_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/shoulder.stl')
-        self.mesh_base_link.apply_scale(2)
-        self.bounding_box = self.mesh_base_link.bounding_box
+        # Load the STL files
+        self.mesh_shulder_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/shoulder.stl')
+        self.mesh_base_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/base.stl')
+        self.mesh_forearm_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/forearm.stl')
+        self.mesh_upperarm_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/upperarm.stl')
+        self.mesh_wrist1_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/wrist1.stl')
+        self.mesh_wrist2_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/wrist2.stl')
+        self.mesh_wrist3_link = trimesh.load_mesh('/home/sochi/robot-sensor/workspaces/COLCON_WS/src/ur_description/meshes/ur10e/collision/wrist3.stl')
+        
+        # Rotate the meshes
+        #rotation_angle = np.radians(90)  # 45 degrees rotation
+        #self.mesh_upperarm_link.apply_transform(trimesh.transformations.rotation_matrix(rotation_angle, [1, 0, 0]))
+
+        self.meshes = [
+            self.mesh_shulder_link,
+            self.mesh_base_link,
+            self.mesh_forearm_link,
+            self.mesh_upperarm_link,
+            self.mesh_wrist1_link,
+            self.mesh_wrist2_link,
+            self.mesh_wrist3_link
+        ]
+
+        for mesh in self.meshes:
+            mesh.apply_scale(1.5)
+
+                # Visualize the meshes
+        #self.visualize_meshes()
+
+    def visualize_meshes(self):
+        scene = trimesh.Scene()
+        for mesh in self.meshes:
+            scene.add_geometry(mesh)
+        scene.show()
 
     def pointcloud_callback(self, msg):
-        self.perspective_frame ='shoulder_link'
+        self.perspective_frame ='upper_arm_link'
         try:
             now = rclpy.time.Time().to_msg()
-            trans = self.tf_buffer.lookup_transform(self.perspective_frame, msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
+            trans_shoulder = self.tf_buffer.lookup_transform('shoulder_link', msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
+            trans_forearm = self.tf_buffer.lookup_transform('forearm_link', msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
+            trans_base = self.tf_buffer.lookup_transform('base_link', msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
+            trans_upperarm = self.tf_buffer.lookup_transform('upper_arm_link', msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
+            trans_wrist1 = self.tf_buffer.lookup_transform('wrist_1_link', msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
+            trans_wrist2 = self.tf_buffer.lookup_transform('wrist_2_link', msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
+            trans_wrist3 = self.tf_buffer.lookup_transform('wrist_3_link', msg.header.frame_id, now, timeout=rclpy.duration.Duration(seconds=1.0))
         except Exception as e:
             self.get_logger().error(f'Error in looking up transform: {e}')
             return
         
-        self.transformed_points = self.do_transform_cloud(msg, trans.transform)
-        inside = self.mesh_base_link.contains(self.transformed_points)
-        outside = np.logical_not(inside)
-        self.transformed_points =np.asanyarray(self.transformed_points)
-        points_valid = np.hstack((self.transformed_points, inside.reshape(256,1)))
-        print(len(self.transformed_points[inside]))
-        self.publish_point_cloud(self.transformed_points[outside], self.transformed_points[inside])
+        self.transformed_points_shoulder = self.do_transform_cloud(msg, trans_shoulder.transform)
+        self.transformed_points_forearm = self.do_transform_cloud(msg, trans_forearm.transform)
+        self.transformed_points_base = self.do_transform_cloud(msg, trans_base.transform)
+        self.transformed_points_upperarm = self.do_transform_cloud(msg, trans_upperarm.transform)
+        self.transformed_points_wrist1 = self.do_transform_cloud(msg, trans_wrist1.transform)
+        self.transformed_points_wrist2 = self.do_transform_cloud(msg, trans_wrist2.transform)
+        self.transformed_points_wrist3 = self.do_transform_cloud(msg, trans_wrist3.transform)
+
+        inside_base = self.mesh_base_link.contains(self.transformed_points_base)
+        inside_forearm = self.mesh_forearm_link.contains(self.transformed_points_forearm)
+        inside_upperarm = self.mesh_upperarm_link.contains(self.transformed_points_upperarm)
+        inside_wrist1 = self.mesh_wrist1_link.contains(self.transformed_points_wrist1)
+        inside_wrist2 = self.mesh_wrist2_link.contains(self.transformed_points_wrist2)
+        inside_wrist3 = self.mesh_wrist3_link.contains(self.transformed_points_wrist3)
+        inside_shoulder = self.mesh_shulder_link.contains(self.transformed_points_shoulder)
+        inside_robot = np.logical_or.reduce((
+            inside_base,
+            inside_forearm,
+            inside_upperarm,
+            inside_wrist1,
+            inside_wrist2,
+            inside_wrist3,
+            inside_shoulder
+        ))
+        outside_robot = np.logical_not(inside_robot)
+        self.transformed_points_shoulder =np.asanyarray(self.transformed_points_shoulder)
+        points_valid = np.hstack((self.transformed_points_shoulder, inside_shoulder.reshape(256,1)))
+        self.publish_point_cloud(self.transformed_points_upperarm[outside_robot], self.transformed_points_upperarm[inside_robot])
 
     def do_transform_cloud(self, cloud, trans):
         # Convert the cloud to a numpy array