sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-13 21:48:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

Bunch of Fixes

Browse Source 
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
This commit is contained in:
Sravan Balaji
2020-04-26 19:51:26 -04:00
parent bcac9feeec
commit 733f637d08
9 changed files with 101 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
docs/gear.md
View File

@@ -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`

2
src/rrrobot_ws/src/rrrobot/include/arm_representation.h
View File

@@ -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);

17
src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh
View File

@@ -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
y: -1
z: 1.5
x: 1.22003614902
y: 1.22740316391
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
z: 1.5
y: 0.383
z: 1.0
orientation:
x: 0.0
y: 0.0
y: 0.707
z: 0.0
w: 0.0" \
w: 0.707" \
-1

77
src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
View File

@@ -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());
KDL::SetToZero(arm_current_joint_states_);
// arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
// 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(arm->getChain()->getNrOfJoints());
KDL::SetToZero(joint_positions);
KDL::JntArray joint_positions(num_joints);
// 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");

14
src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp
View File

@@ -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;
}

2
src/rrrobot_ws/src/rrrobot/src/cv_model.py
View File

@@ -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' ,

16
src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp
View File

@@ -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.y = 0;
q.z = 0.63;
q.w = 0.44;
q.x = 0.707;
q.y = 0.0;
q.z = 0.0;
q.w = 0.707;
}
pose.position = p;

2
src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp
View File

@@ -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)
{

8
src/rrrobot_ws/src/rrrobot/test/test_arm.cpp
View File

@@ -36,11 +36,7 @@
#include "arm_representation.h"
// using namespace std;
using std::cin;
using std::cout;
using std::endl;
using std::string;
using namespace std;
// 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)
{