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

Arm Workflow Working & Comment Cleanup

Browse Source 
- Remove extra points after decimal and slightly increase z for arm_pub_test
- Increase rotation threshold to 0.05 for comparing quaternion orientations
- Remove TODO comments
- Fix z offset direction when correcting position if item doesn't attach
- Remove commented out code from test_arm
This commit is contained in:
Sravan Balaji
2020-04-26 21:12:24 -04:00
parent c4969bbb4c
commit b761daf3b3
6 changed files with 11 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,9 +4,9 @@ source /app/rrrobot_ws/devel/setup.bash
rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
position:
x: 1.22003614902
y: 1.22740316391
z: 0.97
x: 1.22
y: 1.22
z: 0.9725
orientation:
x: 0.0
y: 0.707

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

@@ -355,7 +355,7 @@ private:
arm_joint_trajectory_publisher_.publish(msg);
// TODO: Wait to reach target
// Wait to reach target
ros::Duration((num_points+1)*time_per_step).sleep();
}
@@ -375,9 +375,9 @@ private:
bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
{
// TODO: Tune threshold values
// Tune threshold values
float pos_thresh = 0.15; // Meters
float rot_thresh = 0.025;
float rot_thresh = 0.05;
float pos_err = fabs(cur.position.x - target.position.x) +
fabs(cur.position.y - target.position.y) +
@@ -436,7 +436,7 @@ int main(int argc, char **argv)
double time_per_step = 3.0; // seconds
int retry_attempts = 3;
double item_attach_z_adjustment = 0.025; // meters
double item_attach_z_adjustment = 0.05; // meters
ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment);
ros::AsyncSpinner spinner(0);

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

@@ -271,18 +271,16 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
int main(int argc, char **argv)
{
// TODO: Subscribe to depth camera topic
// Last argument is the default name of the node.
ros::init(argc, argv, "depth_camera_node");
ros::NodeHandle node;
// Subscribe to depth camera topic
ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
// Publish object's current location to topic for RRRobot node to listen to
pub = node.advertise<geometry_msgs::Pose>(DESIRED_GRASP_POSE_CHANNEL, 1);
ros::spin();
// TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item
// TODO: Publish object's current location to topic for RRRobot node to listen to
}

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

@@ -99,7 +99,7 @@ private:
image_file_msg.data = models[rand_idx].image_file;
cout << "Spawning " << msg.model_name << endl;
// TODO: Determine position to spawn items
// Determine position to spawn items
// can place objects on conveyor belt just upstream from the arms
// at roughly (0.2516105, 5.474367, 0.935669)
// x range: 1.022548-1.413952

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

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

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

@@ -38,124 +38,6 @@
using namespace std;
// class ArmRepresentation
// {
// public:
// ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1)))
// {
// const double base_len = 0.2;
// const double shoulder_height = 0.1273;
// const double upper_arm_length = 0.612;
// const double forearm_length = 0.5723;
// const double shoulder_offset = 0.220941;
// const double elbow_offset = -0.1719;
// const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset;
// const double wrist_2_length = 0.1157;
// const double wrist_3_length = 0.0922;
// // KDL::Vector pos; //(0, 0, base_len/2);
// // KDL::Rotation rot;
// // The joints might be off by one
// KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
// KDL::Joint(KDL::Joint::JointType::None), base_pose);
// arm.addSegment(linear_arm_actuator);
// const KDL::Vector pos_base(0, 0, base_len / 2);
// const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0));
// KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::TransY),
// KDL::Frame(rot_base, pos_base));
// arm.addSegment(base_link);
// const KDL::Vector pos_shoulder(0, 0, shoulder_height);
// const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0));
// KDL::Segment shoulder_link("shoulder_link", KDL::Joint(KDL::Joint::JointType::RotZ),
// KDL::Frame(rot_shoulder, pos_shoulder));
// arm.addSegment(shoulder_link);
// const KDL::Vector pos_upper_arm(0, shoulder_offset, 0);
// const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
// KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_upper_arm, pos_upper_arm));
// arm.addSegment(upper_arm_link);
// const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length);
// const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment forearm_link("forearm_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_forearm, pos_forearm));
// arm.addSegment(forearm_link);
// const KDL::Vector pos_wrist_1(0, 0, forearm_length);
// const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0));
// KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_wrist_1, pos_wrist_1));
// arm.addSegment(wrist_1_link);
// const KDL::Vector pos_wrist_2(0, wrist_1_length, 0);
// const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint(KDL::Joint::JointType::RotZ),
// KDL::Frame(rot_wrist_2, pos_wrist_2));
// arm.addSegment(wrist_2_link);
// const KDL::Vector pos_wrist_3(0, 0, wrist_2_length);
// const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0));
// KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint(KDL::Joint::JointType::RotY),
// KDL::Frame(rot_wrist_3, pos_wrist_3));
// arm.addSegment(wrist_3_link);
// const KDL::Vector pos_ee(0, wrist_3_length, 0.0);
// const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0));
// KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
// KDL::Frame(rot_ee, pos_ee));
// arm.addSegment(ee_link);
// // arm.addSegment(base_link);
// // arm.addSegment(shoulder_link);
// // arm.addSegment(upper_arm_link);
// // arm.addSegment(forearm_link);
// // arm.addSegment(wrist_1_link);
// // arm.addSegment(wrist_2_link);
// // arm.addSegment(wrist_3_link);
// // arm.addSegment(ee_link);
// fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
// ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
// }
// int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
// {
// return fk_solver->JntToCart(joint_positions, end_effector_pose);
// }
// int calculateInverseKinematics(const KDL::JntArray &cur_configuration,
// const KDL::Frame &desired_end_effector_pose,
// KDL::JntArray &final_joint_configuration)
// {
// return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
// }
// string *get_joint_names()
// {
// // TODO: return ["linear_arm_actuator_joint",
// // "shoulder_pan_joint",
// // "shoulder_lift_joint",
// // "elbow_joint",
// // "wrist_1_joint",
// // "wrist_2_joint",
// // "wrist_3_joint"];
// }
// const KDL::Chain &getArm() const
// {
// return arm;
// }
// private:
// KDL::Chain arm;
// std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
// std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
// };
int main(int argc, char **argv)
{