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

Update inverse kinematics solver parameters. This makes the solver finish without errors

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-25 22:44:41 -04:00
parent 21d20ec5a1
commit 2a85cdee8d
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -40,7 +40,7 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
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::Quaternion(0, sqrt(2)/2, 0, sqrt(2)/2)); 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);
@@ -52,7 +52,7 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
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::Quaternion(0, sqrt(2)/2, 0, sqrt(2)/2)); 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);
@@ -70,7 +70,7 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
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::Quaternion(0, 0, sqrt(2)/2, sqrt(2)/2)); 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);
@@ -93,7 +93,7 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose) int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose)
{ {
KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain); KDL::ChainFkSolverPos_recursive fk_solver(chain);
int status = fk_solver.JntToCart(joint_positions, end_effector_pose); int status = fk_solver.JntToCart(joint_positions, end_effector_pose);
return status; return status;
@@ -103,7 +103,7 @@ int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_confi
const KDL::Frame &desired_end_effector_pose, const KDL::Frame &desired_end_effector_pose,
KDL::JntArray &final_joint_configuration) KDL::JntArray &final_joint_configuration)
{ {
KDL::ChainIkSolverPos_LMA ik_solver = KDL::ChainIkSolverPos_LMA(chain); 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); int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration);
return status; return status;