Skip to content
Snippets Groups Projects
Commit 3ab28b11 authored by Simon Gabl's avatar Simon Gabl
Browse files

Update Cartesian impedance example controller.

parent bc6cfc83
Branches
Tags
No related merge requests found
...@@ -27,15 +27,16 @@ def publisherCallback(msg, arm_id): ...@@ -27,15 +27,16 @@ def publisherCallback(msg, arm_id):
def franka_state_callback(msg): def franka_state_callback(msg):
initial_quaternion = \ initial_quaternion = \
tf.transformations.quaternion_from_matrix( tf.transformations.quaternion_from_matrix(
np.transpose(np.reshape(msg.O_T_EE, np.transpose(np.reshape(msg.O_T_EE_d,
(4, 4)))) (4, 4))))
initial_quaternion = initial_quaternion / np.linalg.norm(initial_quaternion)
marker_pose.pose.orientation.x = initial_quaternion[0] marker_pose.pose.orientation.x = initial_quaternion[0]
marker_pose.pose.orientation.y = initial_quaternion[1] marker_pose.pose.orientation.y = initial_quaternion[1]
marker_pose.pose.orientation.z = initial_quaternion[2] marker_pose.pose.orientation.z = initial_quaternion[2]
marker_pose.pose.orientation.w = initial_quaternion[3] marker_pose.pose.orientation.w = initial_quaternion[3]
marker_pose.pose.position.x = msg.O_T_EE[12] marker_pose.pose.position.x = msg.O_T_EE_d[12]
marker_pose.pose.position.y = msg.O_T_EE[13] marker_pose.pose.position.y = msg.O_T_EE_d[13]
marker_pose.pose.position.z = msg.O_T_EE[14] marker_pose.pose.position.z = msg.O_T_EE_d[14]
global initial_pose_found global initial_pose_found
initial_pose_found = True initial_pose_found = True
......
...@@ -136,9 +136,9 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/, ...@@ -136,9 +136,9 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
// get state variables // get state variables
franka::RobotState robot_state = state_handle_->getRobotState(); franka::RobotState robot_state = state_handle_->getRobotState();
std::array<double, 7> gravity_array = std::array<double, 7> gravity_array =
model_handle_->getGravity(robot_state.m_load, robot_state.F_x_Cload); model_handle_->getGravity(robot_state.m_total, robot_state.F_x_Ctotal);
std::array<double, 7> coriolis_array = std::array<double, 7> coriolis_array =
model_handle_->getCoriolis(robot_state.I_load, robot_state.m_load, robot_state.F_x_Cload); model_handle_->getCoriolis(robot_state.I_total, robot_state.m_total, robot_state.F_x_Ctotal);
std::array<double, 42> jacobian_array = std::array<double, 42> jacobian_array =
model_handle_->getZeroJacobian(franka::Frame::kEndEffector, robot_state); model_handle_->getZeroJacobian(franka::Frame::kEndEffector, robot_state);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment