Skip to content

Commit

Permalink
Switched from euler angles to rotation vectors for cartesian motion g…
Browse files Browse the repository at this point in the history 
…eneration. Addresses the issues discussed in pantor#40
  • Loading branch information
Tim Schneider committed Dec 1, 2022
1 parent c934d7f commit e8e8082
Show file tree
Hide file tree
Showing 4 changed files with 43 additions and 32 deletions.
2 changes: 1 addition & 1 deletion affx
Submodule affx updated 1 files
+32 −1 include/affx/affine.hpp
8 changes: 4 additions & 4 deletions include/frankx/motion_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,16 +17,16 @@ namespace frankx {
using Vector7d = Eigen::Matrix<double, 7, 1>;

struct MotionGenerator {
static inline franka::CartesianPose CartesianPose(const Vector7d& vector, bool include_elbow = true) {
auto affine = Affine(vector);
static inline franka::CartesianPose CartesianPose(const Vector7d& vector, bool include_elbow = true, const Affine& base_frame = Affine()) {
auto affine = base_frame * Affine::fromPosRotVec(vector);
if (include_elbow) {
return franka::CartesianPose(affine.array(), {vector[6], -1});
}
return franka::CartesianPose(affine.array());
}

static inline franka::CartesianPose CartesianPose(const std::array<double, 7>& vector, bool include_elbow = true) {
auto affine = Affine(vector);
static inline franka::CartesianPose CartesianPose(const std::array<double, 7>& vector, bool include_elbow = true, const Affine& base_frame = Affine()) {
auto affine = base_frame * Affine::fromPosRotVec(vector);
if (include_elbow) {
return franka::CartesianPose(affine.array(), {vector[6], -1});
}
Expand Down
61 changes: 36 additions & 25 deletions include/frankx/motion_waypoint_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@ struct WaypointMotionGenerator: public MotionGenerator {
std::vector<Waypoint>::iterator waypoint_iterator;
bool waypoint_has_elbow {false};

Vector7d old_vector;
Affine old_affine;
double old_elbow;
double time {0.0};
Expand All @@ -34,6 +33,8 @@ struct WaypointMotionGenerator: public MotionGenerator {
WaypointMotion& motion;
MotionData& data;

Affine initial_pose;

bool set_target_at_zero_time {true};

const size_t cooldown_iterations {5};
Expand All @@ -49,17 +50,25 @@ struct WaypointMotionGenerator: public MotionGenerator {

void init(const franka::RobotState& robot_state, franka::Duration period) {
franka::CartesianPose initial_cartesian_pose(robot_state.O_T_EE_c, robot_state.elbow_c);
Affine initial_pose(initial_cartesian_pose.O_T_EE);
initial_pose = Affine(initial_cartesian_pose.O_T_EE);

Vector7d initial_vector_initial_pose_frame = (Vector7d()
<< 0, 0, 0, 0, 0, 0, initial_cartesian_pose.elbow[0]).finished();
auto linear_vel_base_frame = (Eigen::Vector3d()
<< robot_state.O_dP_EE_c[0], robot_state.O_dP_EE_c[1], robot_state.O_dP_EE_c[2]).finished();
auto angular_vel_base_frame = (Eigen::Vector3d()
<< robot_state.O_dP_EE_c[2], robot_state.O_dP_EE_c[3], robot_state.O_dP_EE_c[4]).finished();
auto linear_vel_initial_pose_frame = initial_pose.rotation().transpose() * linear_vel_base_frame;
auto angular_vel_initial_pose_frame = initial_pose.rotation().transpose() * angular_vel_base_frame;

Vector7d initial_vector = initial_pose.vector_with_elbow(initial_cartesian_pose.elbow[0]);
Vector7d initial_velocity = (Vector7d() << robot_state.O_dP_EE_c[0], robot_state.O_dP_EE_c[1], robot_state.O_dP_EE_c[2], robot_state.O_dP_EE_c[3], robot_state.O_dP_EE_c[4], robot_state.O_dP_EE_c[5], robot_state.delbow_c[0]).finished();
Vector7d initial_velocity_initial_pose_frame = (Vector7d()
<< linear_vel_initial_pose_frame, angular_vel_initial_pose_frame, robot_state.delbow_c[0]).finished();

old_affine = initial_pose;
old_vector = initial_vector;
old_elbow = old_vector(6);
old_elbow = initial_vector_initial_pose_frame(6);

input_para.current_position = toStd(initial_vector);
input_para.current_velocity = toStd(initial_velocity);
input_para.current_position = toStd(initial_vector_initial_pose_frame);
input_para.current_velocity = toStd(initial_velocity_initial_pose_frame);
input_para.current_acceleration = toStd(Vector7d::Zero());

input_para.enabled = MotionGenerator::VectorCartRotElbow(true, true, true);
Expand All @@ -70,16 +79,16 @@ struct WaypointMotionGenerator: public MotionGenerator {

const auto current_waypoint = *waypoint_iterator;
waypoint_has_elbow = current_waypoint.elbow.has_value();
auto target_position_vector = current_waypoint.getTargetVector(frame, old_affine, old_elbow);
auto target_position_vector = current_waypoint.getTargetVector(
frame, old_affine, old_elbow, initial_pose.inverse());

input_para.enabled = {true, true, true, true, true, true, waypoint_has_elbow};
input_para.target_position = toStd(target_position_vector);
input_para.target_velocity = toStd(Vector7d::Zero());
setInputLimits(input_para, robot, current_waypoint, data);

old_affine = current_waypoint.getTargetAffine(frame, old_affine);
old_vector = target_position_vector;
old_elbow = old_vector(6);
old_elbow = target_position_vector(6);
}
}

Expand Down Expand Up @@ -117,23 +126,23 @@ struct WaypointMotionGenerator: public MotionGenerator {
Affine current_pose(current_cartesian_pose.O_T_EE);
auto current_vector = current_pose.vector_with_elbow(current_cartesian_pose.elbow[0]);
old_affine = current_pose;
old_vector = current_vector;
old_elbow = old_vector(6);
old_elbow = current_cartesian_pose.elbow[0];

const auto current_waypoint = *waypoint_iterator;
waypoint_has_elbow = current_waypoint.elbow.has_value();
auto target_position_vector = current_waypoint.getTargetVector(Affine(), old_affine, old_elbow);
auto target_position_vector = current_waypoint.getTargetVector(
Affine(), old_affine, old_elbow, initial_pose.inverse());

input_para.enabled = {true, true, true, true, true, true, waypoint_has_elbow};
input_para.target_position = toStd(target_position_vector);
input_para.target_velocity = toStd(Vector7d::Zero());
setInputLimits(input_para, robot, current_waypoint, data);

old_affine = current_waypoint.getTargetAffine(Affine(), old_affine);
old_vector = target_position_vector;
old_elbow = old_vector(6);
old_elbow = target_position_vector(6);
} else {
return franka::MotionFinished(MotionGenerator::CartesianPose(input_para.current_position, waypoint_has_elbow));
return franka::MotionFinished(MotionGenerator::CartesianPose(
input_para.current_position, waypoint_has_elbow, initial_pose));
}
}
}
Expand All @@ -160,10 +169,11 @@ struct WaypointMotionGenerator: public MotionGenerator {
// Allow cooldown of motion, so that the low-pass filter has time to adjust to target values
if (current_cooldown_iteration < cooldown_iterations) {
current_cooldown_iteration += 1;
return MotionGenerator::CartesianPose(input_para.target_position, waypoint_has_elbow);
return MotionGenerator::CartesianPose(
input_para.target_position, waypoint_has_elbow, initial_pose);
}

return franka::MotionFinished(MotionGenerator::CartesianPose(input_para.target_position, waypoint_has_elbow));
return franka::MotionFinished(MotionGenerator::CartesianPose(
input_para.target_position, waypoint_has_elbow, initial_pose));

} else if (motion.reload) {
current_motion = motion;
Expand All @@ -176,27 +186,28 @@ struct WaypointMotionGenerator: public MotionGenerator {
if (has_new_waypoint) {
const auto current_waypoint = *waypoint_iterator;
waypoint_has_elbow = current_waypoint.elbow.has_value();
auto target_position_vector = current_waypoint.getTargetVector(frame, old_affine, old_elbow);
auto target_position_vector = current_waypoint.getTargetVector(
frame, old_affine, old_elbow, initial_pose.inverse());

input_para.enabled = {true, true, true, true, true, true, waypoint_has_elbow};
input_para.target_position = toStd(target_position_vector);
input_para.target_velocity = toStd(Vector7d::Zero());
setInputLimits<RobotType>(input_para, robot, current_waypoint, data);

old_affine = current_waypoint.getTargetAffine(frame, old_affine);
old_vector = target_position_vector;
old_elbow = old_vector(6);
old_elbow = target_position_vector(6);
}

} else if (result == ruckig::Result::Error) {
std::cout << "[frankx robot] Invalid inputs:" << std::endl;
return franka::MotionFinished(MotionGenerator::CartesianPose(input_para.current_position, waypoint_has_elbow));
return franka::MotionFinished(MotionGenerator::CartesianPose(
input_para.current_position, waypoint_has_elbow, initial_pose));
}

output_para.pass_to_input(input_para);
}

return MotionGenerator::CartesianPose(output_para.new_position, waypoint_has_elbow);
return MotionGenerator::CartesianPose(output_para.new_position, waypoint_has_elbow, initial_pose);
}
};

Expand Down
4 changes: 2 additions & 2 deletions include/movex/waypoint.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,14 +66,14 @@ struct Waypoint {
return getTargetVector(Affine(), old_affine, old_elbow);
}

Vector7d getTargetVector(const Affine& frame, const Affine& old_affine, double old_elbow) const {
Vector7d getTargetVector(const Affine& frame, const Affine& old_affine, double old_elbow, const Affine& base_frame = Affine()) const {
double new_elbow;
if (reference_type == ReferenceType::Relative) {
new_elbow = elbow.value_or(0.0) + old_elbow;
} else {
new_elbow = elbow.value_or(old_elbow);
}
return getTargetAffine(frame, old_affine).vector_with_elbow(new_elbow);
return (base_frame * getTargetAffine(frame, old_affine)).vector_with_elbow(new_elbow);
}
};

Expand Down

0 comments on commit e8e8082

Please sign in to comment.