diff --git a/franka_example_controllers/src/model_example_controller.cpp b/franka_example_controllers/src/model_example_controller.cpp
index 68d37222e7b6168ac921168c47e16a32209a5458..0fadbcce8d5a46d925e8f7cbbbb13c63b33b0a3d 100644
--- a/franka_example_controllers/src/model_example_controller.cpp
+++ b/franka_example_controllers/src/model_example_controller.cpp
@@ -59,10 +59,19 @@ void ModelExampleController::update(const ros::Time& /*time*/, const ros::Durati
std::array<double, 7> coriolis = model_handle_->getCoriolis(
{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, 0.0, {{0.0, 0.0, 0.0}});
std::array<double, 7> gravity = model_handle_->getGravity(0.0, {{0.0, 0.0, 0.0}});
+ std::array<double, 16> pose = model_handle_->getPose(franka::Frame::kJoint4);
+ std::array<double, 42> joint4_body_jacobian =
+ model_handle_->getBodyJacobian(franka::Frame::kJoint4);
+ std::array<double, 42> endeffector_zero_jacobian =
+ model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+
ROS_INFO("--------------------------------------------------");
ROS_INFO_STREAM("mass :" << mass);
ROS_INFO_STREAM("coriolis: " << coriolis);
ROS_INFO_STREAM("gravity :" << gravity);
+ ROS_INFO_STREAM("joint_pose :" << pose);
+ ROS_INFO_STREAM("joint4_body_jacobian :" << joint4_body_jacobian);
+ ROS_INFO_STREAM("joint_zero_jacobian :" << endeffector_zero_jacobian);
}
}
diff --git a/franka_hw/include/franka_hw/franka_model_interface.h b/franka_hw/include/franka_hw/franka_model_interface.h
index fd20993a74a58e367986fc8783ae86f6f82690fe..34be823a53b739696130e788576688b6176d7b92 100644
--- a/franka_hw/include/franka_hw/franka_model_interface.h
+++ b/franka_hw/include/franka_hw/franka_model_interface.h
@@ -15,29 +15,29 @@ class FrankaModelHandle {
FrankaModelHandle() = delete;
/**
- * Constructs an instance of a FrankaModelHandle
- *
- * @param[in] name The name of the handle
- * @param[in] model A reference to a a franka::Model model instance
- * @param[in] robot_state A reference to the storage of the current robot state
- */
+ * Constructs an instance of a FrankaModelHandle
+ *
+ * @param[in] name The name of the handle
+ * @param[in] model A reference to a a franka::Model model instance
+ * @param[in] robot_state A reference to the storage of the current robot state
+ */
FrankaModelHandle(const std::string& name, franka::Model& model, franka::RobotState& robot_state)
: name_(name), model_(&model), robot_state_(&robot_state) {}
/**
- * Returns the resource name of the Handle
- */
+ * Returns the resource name of the Handle
+ */
std::string getName() const { return name_; }
/**
- * Returns the inertia matrix, given the current robot state and given external
- * loads
- *
- * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
- * frame
- * @param[in] load_mass The mass of the load
- * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
- */
+ * Returns the inertia matrix, given the current robot state and given external
+ * loads
+ *
+ * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
+ * frame
+ * @param[in] load_mass The mass of the load
+ * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
+ */
std::array<double, 49> getMass(
const std::array<double, 9>& load_inertia,
double load_mass,
@@ -46,15 +46,15 @@ class FrankaModelHandle {
}
/**
- * Returns the inertia matrix, given the input robot state and given external
- * loads
- *
- * @param[in] robot_state A user-given robot state to evaluate the dynamics
- * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
- * frame
- * @param[in] load_mass The mass of the load
- * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
- */
+ * Returns the inertia matrix, given the input robot state and given external
+ * loads
+ *
+ * @param[in] robot_state A user-given robot state to evaluate the dynamics
+ * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
+ * frame
+ * @param[in] load_mass The mass of the load
+ * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
+ */
std::array<double, 49> getMass(
const franka::RobotState& robot_state,
const std::array<double, 9>& load_inertia,
@@ -64,14 +64,14 @@ class FrankaModelHandle {
}
/**
- * Returns the coriolis torques given the current robot state and given
- * external loads
- *
- * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
- * frame
- * @param[in] load_mass The mass of the load
- * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
- */
+ * Returns the coriolis torques given the current robot state and given
+ * external loads
+ *
+ * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
+ * frame
+ * @param[in] load_mass The mass of the load
+ * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
+ */
std::array<double, 7> getCoriolis(
const std::array<double, 9>& load_inertia,
double load_mass,
@@ -80,15 +80,15 @@ class FrankaModelHandle {
}
/**
- * Returns the coriolis torques, given the input robot state and given external
- * loads
- *
- * @param[in] robot_state A user-given robot state to evaluate the dynamics
- * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
- * frame
- * @param[in] load_mass The mass of the load
- * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
- */
+ * Returns the coriolis torques, given the input robot state and given external
+ * loads
+ *
+ * @param[in] robot_state A user-given robot state to evaluate the dynamics
+ * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
+ * frame
+ * @param[in] load_mass The mass of the load
+ * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
+ */
std::array<double, 7> getCoriolis(
const franka::RobotState& robot_state,
const std::array<double, 9>& load_inertia,
@@ -98,14 +98,14 @@ class FrankaModelHandle {
}
/**
- * Returns the gravity torques, given the current robot state and given
- * external loads
- *
- * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
- * frame
- * @param[in] load_mass The mass of the load
- * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
- */
+ * Returns the gravity torques, given the current robot state and given
+ * external loads
+ *
+ * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
+ * frame
+ * @param[in] load_mass The mass of the load
+ * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
+ */
std::array<double, 7> getGravity(
double load_mass,
const std::array<double, 3>& F_x_Cload, // NOLINT (readability-identifier-naming)
@@ -114,15 +114,15 @@ class FrankaModelHandle {
}
/**
- * Returns the gravity torques, given the input robot state and given external
- * loads
- *
- * @param[in] robot_state A user-given robot state to evaluate the dynamics
- * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
- * frame
- * @param[in] load_mass The mass of the load
- * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
- */
+ * Returns the gravity torques, given the input robot state and given external
+ * loads
+ *
+ * @param[in] robot_state A user-given robot state to evaluate the dynamics
+ * @param[in] load_inertia The column major inertia tensor for a load w.r.t. F
+ * frame
+ * @param[in] load_mass The mass of the load
+ * @param[in] F_x_Cload The center of mass of the load w.r.t. F-frame
+ */
std::array<double, 7> getGravity(
const franka::RobotState& robot_state,
double load_mass,
@@ -131,6 +131,82 @@ class FrankaModelHandle {
return model_->gravity(robot_state, load_mass, F_x_Cload, gravity_earth);
}
+ /**
+ * Returns the Cartesian pose matrix of a frame w.r.t. the O-frame of the robot
+ *
+ * @param[in] frame The desired frame.
+ * @param[in] robot_state A user-given robot state to evaluate the forward kinematics.
+ *
+ * @return Vectorized 4x4 homogeneous transform, column-major.
+ */
+ std::array<double, 16> getPose(const franka::Frame& frame,
+ const franka::RobotState& robot_state) {
+ return model_->pose(frame, robot_state);
+ }
+
+ /**
+ * Returns the Cartesian pose matrix of a frame w.r.t. the O-frame of the robot
+ *
+ * @param[in] frame The desired frame.
+ *
+ * @return Vectorized 4x4 homogeneous transform, column-major.
+ */
+ std::array<double, 16> getPose(const franka::Frame& frame) {
+ return getPose(frame, *robot_state_);
+ }
+
+ /**
+ * Gets the 6x7 Jacobian for the given frame w.r.t. that frame. The Jacobian is represented
+ * as a 6x7 matrix in column-major format and is evaluated at the given robot state
+ *
+ * @param[in] frame The desired frame.
+ * @param[in] robot_state State from which the pose should be calculated.
+ *
+ * @return Vectorized 6x7 Jacobian, column-major.
+ */
+ std::array<double, 42> getBodyJacobian(const franka::Frame& frame,
+ const franka::RobotState& robot_state) {
+ return model_->bodyJacobian(frame, robot_state);
+ }
+
+ /**
+ * Gets the 6x7 Jacobian for the given frame w.r.t. that frame and for the current robot
+ * state. The Jacobian is represented as a 6x7 matrix in column-major format.
+ *
+ * @param[in] frame The desired frame.
+ *
+ * @return Vectorized 6x7 Jacobian, column-major.
+ */
+ std::array<double, 42> getBodyJacobian(const franka::Frame& frame) {
+ return getBodyJacobian(frame, *robot_state_);
+ }
+
+ /**
+ * Gets the 6x7 Jacobian for the given frame w.r.t. the link0 frame. The Jacobian is represented
+ * as a 6x7 matrix in column-major format and is evaluated at the given robot state
+ *
+ * @param[in] frame The desired frame.
+ * @param[in] robot_state State from which the pose should be calculated.
+ *
+ * @return Vectorized 6x7 Jacobian, column-major.
+ */
+ std::array<double, 42> getZeroJacobian(const franka::Frame& frame,
+ const franka::RobotState& robot_state) {
+ return model_->zeroJacobian(frame, robot_state);
+ }
+
+ /**
+ * Gets the 6x7 Jacobian for the given frame w.r.t. the link0 frame and for the current robot
+ * state. The Jacobian is represented as a 6x7 matrix in column-major format.
+ *
+ * @param[in] frame The desired frame.
+ *
+ * @return Vectorized 6x7 Jacobian, column-major.
+ */
+ std::array<double, 42> getZeroJacobian(const franka::Frame& frame) {
+ return getZeroJacobian(frame, *robot_state_);
+ }
+
private:
std::string name_;
const franka::Model* model_;
diff --git a/franka_hw/msg/Errors.msg b/franka_hw/msg/Errors.msg
index d3866c653c44176f0218b87cb2e0c03f25054d89..cddeed88f7e33a89a1a968150725803482744025 100644
--- a/franka_hw/msg/Errors.msg
+++ b/franka_hw/msg/Errors.msg
@@ -21,4 +21,13 @@ bool cartesian_motion_generator_velocity_discontinuity
bool cartesian_motion_generator_acceleration_discontinuity
bool cartesian_motion_generator_elbow_sign_inconsistent
bool cartesian_motion_generator_start_elbow_invalid
+bool cartesian_motion_generator_joint_position_limits_violation
+bool cartesian_motion_generator_joint_velocity_limits_violation
+bool cartesian_motion_generator_joint_velocity_discontinuity
+bool cartesian_motion_generator_joint_acceleration_discontinuity
+bool cartesian_position_motion_generator_invalid_frame
bool force_controller_desired_force_tolerance_violation
+bool controller_torque_discontinuity
+bool start_elbow_sign_inconsistent
+bool communication_constraints_violation
+bool power_limit_violation
diff --git a/franka_hw/src/franka_hw_node.cpp b/franka_hw/src/franka_hw_node.cpp
index e1e34b6ecec74382912646363cf01e09c46c8f17..04616950aed0cfad4dffecf6c9198f0916301d42 100644
--- a/franka_hw/src/franka_hw_node.cpp
+++ b/franka_hw/src/franka_hw_node.cpp
@@ -103,8 +103,8 @@ int main(int argc, char** argv) {
recovery_action_server.start();
- // Start background thread for message handling
- ros::AsyncSpinner spinner(1);
+ // Start background threads for message handling
+ ros::AsyncSpinner spinner(4);
spinner.start();
while (ros::ok()) {
diff --git a/franka_hw/src/franka_state_controller.cpp b/franka_hw/src/franka_state_controller.cpp
index 40858bb74f100ee824cf27702927633de86c9a96..1f1a68302ae4a4927c43732ec51ef6de1379e290 100644
--- a/franka_hw/src/franka_state_controller.cpp
+++ b/franka_hw/src/franka_state_controller.cpp
@@ -25,74 +25,100 @@ tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
franka_hw::Errors errorsToMessage(const franka::Errors& error) {
franka_hw::Errors message;
- message.cartesian_motion_generator_acceleration_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
- error.cartesian_motion_generator_acceleration_discontinuity);
- message.cartesian_motion_generator_elbow_limit_violation =
- static_cast<decltype(message.cartesian_motion_generator_elbow_limit_violation)>(
- error.cartesian_motion_generator_elbow_limit_violation);
- message.cartesian_motion_generator_elbow_sign_inconsistent =
- static_cast<decltype(message.cartesian_motion_generator_elbow_sign_inconsistent)>(
- error.cartesian_motion_generator_elbow_sign_inconsistent);
- message.cartesian_motion_generator_start_elbow_invalid =
- static_cast<decltype(message.cartesian_motion_generator_start_elbow_invalid)>(
- error.cartesian_motion_generator_start_elbow_invalid);
- message.cartesian_motion_generator_velocity_discontinuity =
- static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
- error.cartesian_motion_generator_velocity_discontinuity);
- message.cartesian_motion_generator_velocity_limits_violation =
- static_cast<decltype(message.cartesian_motion_generator_velocity_limits_violation)>(
- error.cartesian_motion_generator_velocity_limits_violation);
+ message.joint_position_limits_violation =
+ static_cast<decltype(message.joint_position_limits_violation)>(
+ error.joint_position_limits_violation);
message.cartesian_position_limits_violation =
static_cast<decltype(message.cartesian_position_limits_violation)>(
error.cartesian_position_limits_violation);
- message.cartesian_position_motion_generator_start_pose_invalid =
- static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>(
- error.cartesian_position_motion_generator_start_pose_invalid);
- message.cartesian_reflex =
- static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
- message.cartesian_velocity_profile_safety_violation =
- static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
- error.cartesian_velocity_profile_safety_violation);
+ message.self_collision_avoidance_violation =
+ static_cast<decltype(message.self_collision_avoidance_violation)>(
+ error.self_collision_avoidance_violation);
+ message.joint_velocity_violation =
+ static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
message.cartesian_velocity_violation =
static_cast<decltype(message.cartesian_velocity_violation)>(
error.cartesian_velocity_violation);
- message.force_controller_desired_force_tolerance_violation =
- static_cast<decltype(message.force_controller_desired_force_tolerance_violation)>(
- error.force_controller_desired_force_tolerance_violation);
message.force_control_safety_violation =
static_cast<decltype(message.force_control_safety_violation)>(
error.force_control_safety_violation);
- message.joint_motion_generator_acceleration_discontinuity =
- static_cast<decltype(message.joint_motion_generator_acceleration_discontinuity)>(
- error.joint_motion_generator_acceleration_discontinuity);
- message.joint_motion_generator_position_limits_violation =
- static_cast<decltype(message.joint_motion_generator_position_limits_violation)>(
- error.joint_motion_generator_position_limits_violation);
- message.joint_motion_generator_velocity_discontinuity =
- static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
- error.joint_motion_generator_velocity_discontinuity);
- message.joint_motion_generator_velocity_limits_violation =
- static_cast<decltype(message.joint_motion_generator_velocity_limits_violation)>(
- error.joint_motion_generator_velocity_limits_violation);
- message.joint_position_limits_violation =
- static_cast<decltype(message.joint_position_limits_violation)>(
- error.joint_position_limits_violation);
- message.joint_position_motion_generator_start_pose_invalid =
- static_cast<decltype(message.joint_position_motion_generator_start_pose_invalid)>(
- error.joint_position_motion_generator_start_pose_invalid);
message.joint_reflex = static_cast<decltype(message.joint_reflex)>(error.joint_reflex);
- message.joint_velocity_violation =
- static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
+ message.cartesian_reflex =
+ static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
message.max_goal_pose_deviation_violation =
static_cast<decltype(message.max_goal_pose_deviation_violation)>(
error.max_goal_pose_deviation_violation);
message.max_path_pose_deviation_violation =
static_cast<decltype(message.max_path_pose_deviation_violation)>(
error.max_path_pose_deviation_violation);
- message.self_collision_avoidance_violation =
- static_cast<decltype(message.self_collision_avoidance_violation)>(
- error.self_collision_avoidance_violation);
+ message.cartesian_velocity_profile_safety_violation =
+ static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
+ error.cartesian_velocity_profile_safety_violation);
+ message.joint_position_motion_generator_start_pose_invalid =
+ static_cast<decltype(message.joint_position_motion_generator_start_pose_invalid)>(
+ error.joint_position_motion_generator_start_pose_invalid);
+ message.joint_motion_generator_position_limits_violation =
+ static_cast<decltype(message.joint_motion_generator_position_limits_violation)>(
+ error.joint_motion_generator_position_limits_violation);
+ message.joint_motion_generator_velocity_limits_violation =
+ static_cast<decltype(message.joint_motion_generator_velocity_limits_violation)>(
+ error.joint_motion_generator_velocity_limits_violation);
+ message.joint_motion_generator_velocity_discontinuity =
+ static_cast<decltype(message.joint_motion_generator_velocity_discontinuity)>(
+ error.joint_motion_generator_velocity_discontinuity);
+ message.joint_motion_generator_acceleration_discontinuity =
+ static_cast<decltype(message.joint_motion_generator_acceleration_discontinuity)>(
+ error.joint_motion_generator_acceleration_discontinuity);
+ message.cartesian_position_motion_generator_start_pose_invalid =
+ static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>(
+ error.cartesian_position_motion_generator_start_pose_invalid);
+ message.cartesian_motion_generator_elbow_limit_violation =
+ static_cast<decltype(message.cartesian_motion_generator_elbow_limit_violation)>(
+ error.cartesian_motion_generator_elbow_limit_violation);
+ message.cartesian_motion_generator_velocity_limits_violation =
+ static_cast<decltype(message.cartesian_motion_generator_velocity_limits_violation)>(
+ error.cartesian_motion_generator_velocity_limits_violation);
+ message.cartesian_motion_generator_velocity_discontinuity =
+ static_cast<decltype(message.cartesian_motion_generator_velocity_discontinuity)>(
+ error.cartesian_motion_generator_velocity_discontinuity);
+ message.cartesian_motion_generator_acceleration_discontinuity =
+ static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>(
+ error.cartesian_motion_generator_acceleration_discontinuity);
+ message.cartesian_motion_generator_elbow_sign_inconsistent =
+ static_cast<decltype(message.cartesian_motion_generator_elbow_sign_inconsistent)>(
+ error.cartesian_motion_generator_elbow_sign_inconsistent);
+ message.cartesian_motion_generator_start_elbow_invalid =
+ static_cast<decltype(message.cartesian_motion_generator_start_elbow_invalid)>(
+ error.cartesian_motion_generator_start_elbow_invalid);
+ message.cartesian_motion_generator_joint_position_limits_violation =
+ static_cast<decltype(message.cartesian_motion_generator_joint_position_limits_violation)>(
+ error.cartesian_motion_generator_joint_position_limits_violation);
+ message.cartesian_motion_generator_joint_velocity_limits_violation =
+ static_cast<decltype(message.cartesian_motion_generator_joint_velocity_limits_violation)>(
+ error.cartesian_motion_generator_joint_velocity_limits_violation);
+ message.cartesian_motion_generator_joint_velocity_discontinuity =
+ static_cast<decltype(message.cartesian_motion_generator_joint_velocity_discontinuity)>(
+ error.cartesian_motion_generator_joint_velocity_discontinuity);
+ message.cartesian_motion_generator_joint_acceleration_discontinuity =
+ static_cast<decltype(message.cartesian_motion_generator_joint_acceleration_discontinuity)>(
+ error.cartesian_motion_generator_joint_acceleration_discontinuity);
+ message.cartesian_position_motion_generator_invalid_frame =
+ static_cast<decltype(message.cartesian_position_motion_generator_invalid_frame)>(
+ error.cartesian_position_motion_generator_invalid_frame);
+ message.force_controller_desired_force_tolerance_violation =
+ static_cast<decltype(message.force_controller_desired_force_tolerance_violation)>(
+ error.force_controller_desired_force_tolerance_violation);
+ message.controller_torque_discontinuity =
+ static_cast<decltype(message.controller_torque_discontinuity)>(
+ error.controller_torque_discontinuity);
+ message.start_elbow_sign_inconsistent =
+ static_cast<decltype(message.start_elbow_sign_inconsistent)>(
+ error.start_elbow_sign_inconsistent);
+ message.communication_constraints_violation =
+ static_cast<decltype(message.communication_constraints_violation)>(
+ error.communication_constraints_violation);
+ message.power_limit_violation =
+ static_cast<decltype(message.power_limit_violation)>(error.power_limit_violation);
return message;
}