Skip to content
Snippets Groups Projects
Commit 2941ea4e authored by Florian Walch's avatar Florian Walch
Browse files

Merge pull request #37 in SWDEV/franka_ros from rcu-16.0.0 to master

* commit '3537d9016ff55b7308cce96d384d5d8561fd3ee4':
  Remove unnecessary include.
  Increase number of background threads to allow parallel service calls.
  Add new errors, sort in libfranka order.
  Use franka::Frame instead of strings
  added kinematics to model example controller
  added Jacobians and forward kinematics to model interface
parents 43f91474 9271f20f
No related branches found
No related tags found
No related merge requests found
...@@ -59,10 +59,19 @@ void ModelExampleController::update(const ros::Time& /*time*/, const ros::Durati ...@@ -59,10 +59,19 @@ void ModelExampleController::update(const ros::Time& /*time*/, const ros::Durati
std::array<double, 7> coriolis = model_handle_->getCoriolis( 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}}); {{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, 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("--------------------------------------------------");
ROS_INFO_STREAM("mass :" << mass); ROS_INFO_STREAM("mass :" << mass);
ROS_INFO_STREAM("coriolis: " << coriolis); ROS_INFO_STREAM("coriolis: " << coriolis);
ROS_INFO_STREAM("gravity :" << gravity); 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);
} }
} }
......
...@@ -131,6 +131,82 @@ class FrankaModelHandle { ...@@ -131,6 +131,82 @@ class FrankaModelHandle {
return model_->gravity(robot_state, load_mass, F_x_Cload, gravity_earth); 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: private:
std::string name_; std::string name_;
const franka::Model* model_; const franka::Model* model_;
......
...@@ -21,4 +21,13 @@ bool cartesian_motion_generator_velocity_discontinuity ...@@ -21,4 +21,13 @@ bool cartesian_motion_generator_velocity_discontinuity
bool cartesian_motion_generator_acceleration_discontinuity bool cartesian_motion_generator_acceleration_discontinuity
bool cartesian_motion_generator_elbow_sign_inconsistent bool cartesian_motion_generator_elbow_sign_inconsistent
bool cartesian_motion_generator_start_elbow_invalid 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 force_controller_desired_force_tolerance_violation
bool controller_torque_discontinuity
bool start_elbow_sign_inconsistent
bool communication_constraints_violation
bool power_limit_violation
...@@ -103,8 +103,8 @@ int main(int argc, char** argv) { ...@@ -103,8 +103,8 @@ int main(int argc, char** argv) {
recovery_action_server.start(); recovery_action_server.start();
// Start background thread for message handling // Start background threads for message handling
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(4);
spinner.start(); spinner.start();
while (ros::ok()) { while (ros::ok()) {
......
...@@ -25,74 +25,100 @@ tf::Transform convertArrayToTf(const std::array<double, 16>& transform) { ...@@ -25,74 +25,100 @@ tf::Transform convertArrayToTf(const std::array<double, 16>& transform) {
franka_hw::Errors errorsToMessage(const franka::Errors& error) { franka_hw::Errors errorsToMessage(const franka::Errors& error) {
franka_hw::Errors message; franka_hw::Errors message;
message.cartesian_motion_generator_acceleration_discontinuity = message.joint_position_limits_violation =
static_cast<decltype(message.cartesian_motion_generator_acceleration_discontinuity)>( static_cast<decltype(message.joint_position_limits_violation)>(
error.cartesian_motion_generator_acceleration_discontinuity); error.joint_position_limits_violation);
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.cartesian_position_limits_violation = message.cartesian_position_limits_violation =
static_cast<decltype(message.cartesian_position_limits_violation)>( static_cast<decltype(message.cartesian_position_limits_violation)>(
error.cartesian_position_limits_violation); error.cartesian_position_limits_violation);
message.cartesian_position_motion_generator_start_pose_invalid = message.self_collision_avoidance_violation =
static_cast<decltype(message.cartesian_position_motion_generator_start_pose_invalid)>( static_cast<decltype(message.self_collision_avoidance_violation)>(
error.cartesian_position_motion_generator_start_pose_invalid); error.self_collision_avoidance_violation);
message.cartesian_reflex = message.joint_velocity_violation =
static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex); static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation);
message.cartesian_velocity_profile_safety_violation =
static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
error.cartesian_velocity_profile_safety_violation);
message.cartesian_velocity_violation = message.cartesian_velocity_violation =
static_cast<decltype(message.cartesian_velocity_violation)>( static_cast<decltype(message.cartesian_velocity_violation)>(
error.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 = message.force_control_safety_violation =
static_cast<decltype(message.force_control_safety_violation)>( static_cast<decltype(message.force_control_safety_violation)>(
error.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_reflex = static_cast<decltype(message.joint_reflex)>(error.joint_reflex);
message.joint_velocity_violation = message.cartesian_reflex =
static_cast<decltype(message.joint_velocity_violation)>(error.joint_velocity_violation); static_cast<decltype(message.cartesian_reflex)>(error.cartesian_reflex);
message.max_goal_pose_deviation_violation = message.max_goal_pose_deviation_violation =
static_cast<decltype(message.max_goal_pose_deviation_violation)>( static_cast<decltype(message.max_goal_pose_deviation_violation)>(
error.max_goal_pose_deviation_violation); error.max_goal_pose_deviation_violation);
message.max_path_pose_deviation_violation = message.max_path_pose_deviation_violation =
static_cast<decltype(message.max_path_pose_deviation_violation)>( static_cast<decltype(message.max_path_pose_deviation_violation)>(
error.max_path_pose_deviation_violation); error.max_path_pose_deviation_violation);
message.self_collision_avoidance_violation = message.cartesian_velocity_profile_safety_violation =
static_cast<decltype(message.self_collision_avoidance_violation)>( static_cast<decltype(message.cartesian_velocity_profile_safety_violation)>(
error.self_collision_avoidance_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; return message;
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment