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

Fix Coordinate Transforms for Arm

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

- Fix base position and orientation relative to world in arm_representation.h
- Update all joint rotation defintions to use Quaternions
This commit is contained in:
Sravan Balaji
2020-04-25 19:26:54 -04:00
parent ed133366d4
commit fc106196d0
2 changed files with 10 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -39,7 +39,7 @@ using std::string;
class ArmRepresentation class ArmRepresentation
{ {
public: public:
ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.0, 0.9))); //KDL::Frame(KDL::Vector(0.3, 0.92, 1))); ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Rotation::Quaternion(0, 0, 0, 1), KDL::Vector(0.3, 0.92, 0.9))); //KDL::Frame(KDL::Vector(0.3, 0.92, 1)));
int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose); int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose);

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

@@ -1,6 +1,7 @@
// arm_controller_node.cpp // arm_controller_node.cpp
#include "arm_representation.h" #include "arm_representation.h"
#include <math.h>
// using namespace std; // using namespace std;
using std::string; using std::string;
@@ -27,49 +28,49 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
chain.addSegment(linear_arm_actuator); chain.addSegment(linear_arm_actuator);
const KDL::Vector pos_base(0, 0, base_len / 2); const KDL::Vector pos_base(0, 0, base_len / 2);
const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0)); const KDL::Rotation rot_base(KDL::Rotation::Quaternion(0, 0, 0, 1));
KDL::Segment base_link("base_link", KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY), KDL::Segment base_link("base_link", KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY),
KDL::Frame(rot_base, pos_base)); KDL::Frame(rot_base, pos_base));
chain.addSegment(base_link); chain.addSegment(base_link);
const KDL::Vector pos_shoulder(0, 0, shoulder_height); const KDL::Vector pos_shoulder(0, 0, shoulder_height);
const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0)); const KDL::Rotation rot_shoulder(KDL::Rotation::Quaternion(0, 0, 0, 1));
KDL::Segment shoulder_link("shoulder_link", KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ), KDL::Segment shoulder_link("shoulder_link", KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ),
KDL::Frame(rot_shoulder, pos_shoulder)); KDL::Frame(rot_shoulder, pos_shoulder));
chain.addSegment(shoulder_link); chain.addSegment(shoulder_link);
const KDL::Vector pos_upper_arm(0, shoulder_offset, 0); 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)); const KDL::Rotation rot_upper_arm(KDL::Rotation::Quaternion(0, sqrt(2)/2, 0, sqrt(2)/2));
KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY), KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY),
KDL::Frame(rot_upper_arm, pos_upper_arm)); KDL::Frame(rot_upper_arm, pos_upper_arm));
chain.addSegment(upper_arm_link); chain.addSegment(upper_arm_link);
const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length); 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)); const KDL::Rotation rot_forearm(KDL::Rotation::Quaternion(0, 0, 0, 1));
KDL::Segment forearm_link("forearm_link", KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY), KDL::Segment forearm_link("forearm_link", KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY),
KDL::Frame(rot_forearm, pos_forearm)); KDL::Frame(rot_forearm, pos_forearm));
chain.addSegment(forearm_link); chain.addSegment(forearm_link);
const KDL::Vector pos_wrist_1(0, 0, forearm_length); 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)); const KDL::Rotation rot_wrist_1(KDL::Rotation::Quaternion(0, sqrt(2)/2, 0, sqrt(2)/2));
KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY), KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY),
KDL::Frame(rot_wrist_1, pos_wrist_1)); KDL::Frame(rot_wrist_1, pos_wrist_1));
chain.addSegment(wrist_1_link); chain.addSegment(wrist_1_link);
const KDL::Vector pos_wrist_2(0, wrist_1_length, 0); 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)); const KDL::Rotation rot_wrist_2(KDL::Rotation::Quaternion(0, 0, 0, 1));
KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ), KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ),
KDL::Frame(rot_wrist_2, pos_wrist_2)); KDL::Frame(rot_wrist_2, pos_wrist_2));
chain.addSegment(wrist_2_link); chain.addSegment(wrist_2_link);
const KDL::Vector pos_wrist_3(0, 0, wrist_2_length); 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)); const KDL::Rotation rot_wrist_3(KDL::Rotation::Quaternion(0, 0, 0, 1));
KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY), KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY),
KDL::Frame(rot_wrist_3, pos_wrist_3)); KDL::Frame(rot_wrist_3, pos_wrist_3));
chain.addSegment(wrist_3_link); chain.addSegment(wrist_3_link);
const KDL::Vector pos_ee(0, wrist_3_length, 0.0); 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)); const KDL::Rotation rot_ee(KDL::Rotation::Quaternion(0, 0, sqrt(2)/2, sqrt(2)/2));
KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None), KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None),
KDL::Frame(rot_ee, pos_ee)); KDL::Frame(rot_ee, pos_ee));
chain.addSegment(ee_link); chain.addSegment(ee_link);