diff --git a/affx b/affx index ee97755f..99cd89eb 160000 --- a/affx +++ b/affx @@ -1 +1 @@ -Subproject commit ee97755f3e5549b388d8d9644356507e0dda6b8f +Subproject commit 99cd89ebeda3447c0ad42e82227a267e19fced29 diff --git a/include/frankx/motion_generator.hpp b/include/frankx/motion_generator.hpp index b1dcb7a1..ee47b83c 100644 --- a/include/frankx/motion_generator.hpp +++ b/include/frankx/motion_generator.hpp @@ -17,16 +17,16 @@ namespace frankx { using Vector7d = Eigen::Matrix; 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& vector, bool include_elbow = true) { - auto affine = Affine(vector); + static inline franka::CartesianPose CartesianPose(const std::array& 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}); } diff --git a/include/frankx/motion_waypoint_generator.hpp b/include/frankx/motion_waypoint_generator.hpp index acc3e5fb..a7b97976 100644 --- a/include/frankx/motion_waypoint_generator.hpp +++ b/include/frankx/motion_waypoint_generator.hpp @@ -24,7 +24,6 @@ struct WaypointMotionGenerator: public MotionGenerator { std::vector::iterator waypoint_iterator; bool waypoint_has_elbow {false}; - Vector7d old_vector; Affine old_affine; double old_elbow; double time {0.0}; @@ -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}; @@ -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); @@ -70,7 +79,8 @@ 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); @@ -78,8 +88,7 @@ struct WaypointMotionGenerator: public MotionGenerator { 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); } } @@ -117,12 +126,12 @@ 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); @@ -130,10 +139,10 @@ struct WaypointMotionGenerator: public MotionGenerator { 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)); } } } @@ -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; @@ -176,7 +186,8 @@ 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); @@ -184,19 +195,19 @@ struct WaypointMotionGenerator: public MotionGenerator { 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); } } 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); } }; diff --git a/include/movex/waypoint.hpp b/include/movex/waypoint.hpp index d8d95c1f..38973c26 100644 --- a/include/movex/waypoint.hpp +++ b/include/movex/waypoint.hpp @@ -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); } };