Bunch of Fixes

Co-authored-by: dwitcpa <dwitcpa@umich.edu>

- Add instructs for running all simulation nodes
- calculateInverseKinematics takes double vector instead of JntArray now
- Fix calls to calculateInverseKinematics
- Fix orientations and bin positions in arm_pub_test.sh
- calc_joint_positions takes start location as argument so we can path plan from intermediate states
- Add some ROS_INFO prints for debugging
- Fix bug in cv_model.py
- Fix orientations sent by depth_camera_node.cpp
- Add a small z offset to desired grasp pose in rrrobot_node before sending to arm controller so end effector doesn't hit object

docs/gear.md

@@ -86,18 +86,38 @@ rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_
 
 ## Running Full Simulation
 
-### ARIAC Environment
+### Terminal 1: ARIAC Environment
 
 - `cd /app/rrrobot_ws/src/rrrobot/scripts`
-- `./rrrobot_run.sh`
+- `./rrrobot_run_no_build.sh`
 
-### Build RRRobot Package
+### Terminal 2: Build RRRobot Package & Arm Controller Node
 
 - `cd /app/rrrobot_ws`
 - `catkin_make clean`
 - `catkin_make`
 - `source devel/setup.bash`
-
-### Arm Controller Node
-
 - `rosrun rrrobot arm_controller_node`
+
+### Terminal 3: CV Model
+
+- `cd /app/rrrobot_ws/src/rrrobot/src/`
+- `python3 cv_model.py`
+
+### Terminal 4: Depth Camera Node (for getting pick up location)
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot depth_camera_node`
+
+### Terminal 5: Run the main rrrobot node
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot rrrobot_node`
+
+### Terminal 6: Run the node to spawn random objects onto the conveyor belt
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot object_spawner_node`

src/rrrobot_ws/src/rrrobot/include/arm_representation.h

@@ -43,7 +43,7 @@ public:
 
     int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr = -1);
 
-    int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
+    int calculateInverseKinematics(const std::vector<double> &cur_configuration,
                                    const KDL::Frame &desired_end_effector_pose,
                                    KDL::JntArray &final_joint_configuration);
 

src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh

@@ -1,11 +1,12 @@
 #!/bin/bash
 
-#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}'
+source /app/rrrobot_ws/devel/setup.bash
+
 rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
   position:
-    x: 1.2
+    x: 1.22003614902
-    y: -1
+    y: 1.22740316391
-    z: 1.5
+    z: 0.97
   orientation:
     x: 0.0
     y: 0.707
@@ -14,11 +15,11 @@ rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
 drop_location:
   position:
     x: -0.3
-    y: 1.15
+    y: 0.383
-    z: 1.5
+    z: 1.0
   orientation:
     x: 0.0
-    y: 0.0
+    y: 0.707
     z: 0.0
-    w: 0.0" \
+    w: 0.707" \
 -1

src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp

@@ -48,10 +48,12 @@ public:
 
         gripper_ = node.serviceClient<osrf_gear::VacuumGripperControl>(GRIPPER_CONTROL_CHANNEL);
 
+        arm_current_joint_states_.resize(arm->getChain()->getNrOfJoints(), 0.0);
+
         // arm = std::move(arm_);
         // ROS_INFO("Arm size (in constructor): %d", arm->getChain()->getNrOfJoints());
-        arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
+        // arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
-        KDL::SetToZero(arm_current_joint_states_);
+        // KDL::SetToZero(arm_current_joint_states_);
     }
 
     void joint_state_callback(const sensor_msgs::JointState &joint_state_msg)
@@ -64,22 +66,9 @@ public:
         vector<string> cur_joint_names = arm->get_joint_names();
         vector<double> position = joint_state_msg.position;
 
-        // Print joint name vectors for debugging
-        // ROS_INFO_STREAM("msg_joint_names: {");
-        // for (int i = 0; i < nbr_joints; ++i)
-        // {
-        //     ROS_INFO_STREAM(msg_joint_names[i] << ", ");
-        // }
-        // ROS_INFO_STREAM("}\ncur_joint_names: {");
-        // for (int j = 0; j < cur_joint_names.size(); ++j)
-        // {
-        //     ROS_INFO_STREAM(cur_joint_names[j] << ", ");
-        // }
-        // ROS_INFO_STREAM("}");
-
         for (int i = 0; i < nbr_joints; ++i)
         {
-            arm_current_joint_states_(i) = 0.0;
+            arm_current_joint_states_[i] = 0.0;
         }
 
         for (size_t i = 0; i < position.size(); ++i)
@@ -88,7 +77,7 @@ public:
             {
                 if (msg_joint_names[i].compare(cur_joint_names[j]) == 0)
                 {
-                    arm_current_joint_states_(j) = position[i];
+                    arm_current_joint_states_[j] = position[i];
                 }
             }
         }
@@ -122,7 +111,7 @@ public:
 
         // Move arm to pickup item from conveyor belt
         attempts = 1;
-        positions = calc_joint_positions(target_pose.grab_location);
+        positions = calc_joint_positions(target_pose.grab_location, above_conveyor);
         send_joint_trajectory(positions, arm_action_phase::bin_to_belt);
 
         // Check if target has been reached
@@ -151,7 +140,7 @@ public:
 
             // If item is not able to attach, adjust z and try again
             target_location.position.z -= item_attach_z_adjustment; // meters
-            positions = calc_joint_positions(target_location);
+            positions = calc_joint_positions(target_location, arm_current_joint_states_);
             send_joint_trajectory(positions, arm_action_phase::item_pickup_adjustment);
             attempts++;
         }
@@ -160,7 +149,7 @@ public:
 
         // Move item to desired position
         attempts = 1;
-        positions = calc_joint_positions(target_pose.drop_location);
+        positions = calc_joint_positions(target_pose.drop_location, above_bins);
         send_joint_trajectory(positions, arm_action_phase::belt_to_bin);
 
         // Check if target has been reached
@@ -198,7 +187,7 @@ public:
                 ROS_ERROR("STEP 6: Item not detaching.. ¯\\_(ツ)_/¯");
             }
             
-            ros::Duration(0.1).sleep();
+            ros::Duration(1).sleep();
             attempts++;
         }
 
@@ -206,11 +195,12 @@ public:
     }
 
 private:
+    // Private variables
     bool gripper_enabled_;
     bool item_attached_;
     ros::Publisher arm_joint_trajectory_publisher_;
     ros::ServiceClient gripper_;
-    KDL::JntArray arm_current_joint_states_;
+    vector<double> arm_current_joint_states_;
     ArmRepresentation *arm;
     enum arm_action_phase {
         belt_to_bin,
@@ -220,8 +210,11 @@ private:
     double time_per_step;
     int retry_attempts;
     double item_attach_z_adjustment;
+    // Define intermediate joint positions
+    const vector<double> above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
+    const vector<double> above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
 
-    vector<double> calc_joint_positions(const geometry_msgs::Pose &pose)
+    vector<double> calc_joint_positions(const geometry_msgs::Pose &pose, const vector<double> &start_state)
     {
         // KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_;
         // KDL::SetToZero(cur_configuration);
@@ -230,7 +223,7 @@ private:
 
         // ROS_INFO("cur_configuration (%i x %i)", cur_configuration.rows(), cur_configuration.columns());
 
-        int error_code = arm->calculateInverseKinematics(arm_current_joint_states_, desired_end_effector_pose, final_joint_configuration);
+        int error_code = arm->calculateInverseKinematics(start_state, desired_end_effector_pose, final_joint_configuration);
 
         // Check status of IK and print error message if there is a failure
         if (error_code != 0)
@@ -253,13 +246,16 @@ private:
 
     KDL::Frame calc_end_effector_pose()
     {
+        int num_joints = arm->getChain()->getNrOfJoints();
+        
         KDL::Frame end_effector_pose;
-        // KDL::JntArray joint_positions = arm_current_joint_states_;
+        KDL::JntArray joint_positions(num_joints);
-        KDL::JntArray joint_positions(arm->getChain()->getNrOfJoints());
+
-        KDL::SetToZero(joint_positions);
+        // Copy current joint states values into JntArray
+        for (int i = 0; i < num_joints; ++i) {
+            joint_positions(i) = arm_current_joint_states_[i];
+        }
 
-        // ROS_INFO("joint_positions (%i x %i)", joint_positions.rows(), joint_positions.columns());
-        // ROS_INFO("arm chain # of joints: %d", arm->getChain()->getNrOfJoints());
         int error_code = arm->calculateForwardKinematics(joint_positions, end_effector_pose);
 
         // Check status of FK and do something if there is a failure
@@ -315,10 +311,6 @@ private:
         // Declare JointTrajectory message
         trajectory_msgs::JointTrajectory msg;
 
-        // Define intermediate joint positions
-        const vector<double> above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
-        const vector<double> above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0};
-
         // Fill the names of the joints to be controlled
         msg.joint_names = arm->get_joint_names();
 
@@ -363,7 +355,7 @@ private:
 
         arm_joint_trajectory_publisher_.publish(msg);
 
-        // Wait to reach target
+        // TODO: Wait to reach target
         ros::Duration((num_points+1)*time_per_step).sleep();
     }
 
@@ -384,13 +376,15 @@ private:
     bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
     {
         // TODO: Tune threshold values
-        float pos_thresh = 0.1; // Meters
+        float pos_thresh = 0.15; // Meters
         float rot_thresh = 0.0875; // Radians
 
         float pos_err = fabs(cur.position.x - target.position.x) +
                         fabs(cur.position.y - target.position.y) +
                         fabs(cur.position.z - target.position.z);
 
+        ROS_INFO_STREAM("pos_err: " << pos_err << " (" << pos_thresh << ")");
+
         if (pos_err > pos_thresh)
         {
             return false;
@@ -403,6 +397,15 @@ private:
         float ry_err = fabs(fmod(rpy1[1] - rpy2[1] + M_PI, M_PI));
         float rz_err = fabs(fmod(rpy1[2] - rpy2[2] + M_PI, M_PI));
 
+        // Errors now between 0 and pi
+        rx_err = min(rx_err, fabs(rx_err - (float) M_PI));
+        ry_err = min(ry_err, fabs(ry_err - (float) M_PI));
+        rz_err = min(rz_err, fabs(rz_err - (float) M_PI));
+
+        ROS_INFO_STREAM("rx_err: " << rx_err << " (" << rot_thresh << ")");
+        ROS_INFO_STREAM("ry_err: " << ry_err << " (" << rot_thresh << ")");
+        ROS_INFO_STREAM("rz_err: " << rz_err << " (" << rot_thresh << ")");
+
         if (rx_err > rot_thresh)
         {
             return false;
@@ -449,9 +452,9 @@ int main(int argc, char **argv)
     // Subscribe to arm destination and joint states channels
     ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac);
 
-    ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 10, &ArmController::gripper_state_callback, &ac);
+    ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 1, &ArmController::gripper_state_callback, &ac);
 
-    ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 10, &ArmController::joint_state_callback, &ac);
+    ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 1, &ArmController::joint_state_callback, &ac);
 
     ROS_INFO("Setup complete");
 

src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp

@@ -99,12 +99,22 @@ int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_pos
     return status;
 }
 
-int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_configuration,
+int ArmRepresentation::calculateInverseKinematics(const vector<double> &cur_configuration,
                                                   const KDL::Frame &desired_end_effector_pose,
                                                   KDL::JntArray &final_joint_configuration)
 {
     KDL::ChainIkSolverPos_LMA ik_solver(chain, 1e-3, 2000, 1e-8);
-    int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
+    
+    // Convert cur_configuration vector to JntArray
+    int num_joints = chain.getNrOfJoints();
+    KDL::JntArray pos(num_joints);
+    KDL::SetToZero(pos);
+
+    for (int i = 0; i < num_joints; ++i) {
+        pos(i) = cur_configuration[i];
+    }
+
+    int status = ik_solver.CartToJnt(pos, desired_end_effector_pose, final_joint_configuration);
 
     return status;
 }

src/rrrobot_ws/src/rrrobot/src/cv_model.py

@@ -38,7 +38,7 @@ def call_back(filename):
     # query model
     predicted_logits = model(input_tensor).squeeze()
     # print(predicted_logits.shape)
-    predicted_label = torch.argmax(predicted_logits).numpy().tolist()
+    predicted_label = torch.argmax(predicted_logits).detach().numpy().tolist()
 
     type_dict = {0: 'cardboard',
                  1: 'glass'    ,

src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp

@@ -218,7 +218,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
 		geometry_msgs::Point p;
 		geometry_msgs::Quaternion q;
 
-		// pick up from top
+		// pick up from top (-z direction)
 		if (area_top > .002)
 		{
 			std::cout << "picking up object from top" << std::endl;
@@ -228,23 +228,23 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
 
 			// rpy = 0 0 0
 			q.x = 0;
-			q.y = 0;
+			q.y = 0.707;
 			q.z = 0;
-			q.w = 1;
+			q.w = 0.707;
 		}
 		else
 		{
-			// pick up from front
+			// pick up from front (+x direction)
 			std::cout << "picking up object from front" << std::endl;
 			p.x = xmin;
 			p.y = (ymin + ymax) / 2;
 			p.z = (zmax + zmin) / 2;
 
 			// rpy = -pi/2 0 pi/2
-			q.x = -0.63;
+			q.x = 0.707;
-			q.y = 0;
+			q.y = 0.0;
-			q.z = 0.63;
+			q.z = 0.0;
-			q.w = 0.44;
+			q.w = 0.707;
 		}
 
 		pose.position = p;

src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp

@@ -118,6 +118,8 @@ public:
 		set_conveyor(0);
 
 		desired_grasp_pose = grasp_pose;
+		// TODO: Tune z offset so end effector doesn't hit object
+		desired_grasp_pose.position.z += 0.01;
 
 		if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
 		{

src/rrrobot_ws/src/rrrobot/test/test_arm.cpp

@@ -36,11 +36,7 @@
 
 #include "arm_representation.h"
 
-// using namespace std;
+using namespace std;
-using std::cin;
-using std::cout;
-using std::endl;
-using std::string;
 
 // class ArmRepresentation
 // {
@@ -190,7 +186,7 @@ int main(int argc, char **argv)
 
             f << end_effector_pose.p.x() << "," << end_effector_pose.p.y() << "," << end_effector_pose.p.z() << '\n';
 
-            error_code = arm.calculateInverseKinematics(pos, end_effector_pose, pos);
+            error_code = arm.calculateInverseKinematics(vector<double>(arm.getChain()->getNrOfJoints(), 0.0), end_effector_pose, pos);
 
             if (error_code != 0)
             {