diff --git a/include/franka_plan_runner.h b/include/franka_plan_runner.h index aa7324f..8552ee9 100755 --- a/include/franka_plan_runner.h +++ b/include/franka_plan_runner.h @@ -55,10 +55,11 @@ #include "drake/lcmt_iiwa_status.hpp" -#include -#include #include +#include +#include #include +#include #include "examples_common.h" @@ -91,7 +92,7 @@ namespace drake { namespace franka_driver { const int kNumJoints = 7; -const std::string home_addr = "192.168.1.1"; +const std::string home_addr = "192.168.1.1"; using trajectories::PiecewisePolynomial; @@ -168,7 +169,7 @@ lcmt_iiwa_status ConvertToLcmStatus(franka::RobotState &robot_state) { robot_status.joint_torque_commanded = ConvertToVector(robot_state.tau_J_d); robot_status.joint_torque_external.resize(num_joints_, 0); - return robot_status; + return robot_status; } void ResizeStatusMessage(lcmt_iiwa_status &lcm_status_) { @@ -205,10 +206,11 @@ int64_t get_current_utime() { return current_utime; } + class FrankaPlanRunner { private: Dracula *dracula = nullptr; - std::string param_yaml_; + std::string param_yaml_; parameters::Parameters p; std::string ip_addr_; std::atomic_bool running_{false}; @@ -249,8 +251,21 @@ class FrankaPlanRunner { std::thread lcm_publish_status_thread; std::thread lcm_handle_thread; + // not used @deprecated + // Eigen::VectorXd integral_error; //make sure this doesn't get destroyed after each call of callback - ask sprax + + // for inv dynamics : + bool run_inverse_dynamics_; + std::unique_ptr> mb_plant_context_; // for multibody plants + drake::multibody::MultibodyPlant mb_plant_ ; + Eigen::VectorXd integral_error; + // PiecewisePolynomial ref_vd_;// = plan_.plan->derivative(2); // might move this into setup + std::string lcm_driver_status_channel_; + Eigen::Matrix pos_start; + bool runonce = true; + public: FrankaPlanRunner(const parameters::Parameters params) : p(params), ip_addr_(params.robot_ip), plan_number_(0), @@ -287,7 +302,7 @@ class FrankaPlanRunner { momap::log()->info("Plan received channel: {}", p.lcm_plan_received_channel); momap::log()->info("Plan complete channel: {}", p.lcm_plan_complete_channel); momap::log()->info("Status channel: {}", p.lcm_status_channel); - + }; ~FrankaPlanRunner() {}; @@ -319,7 +334,7 @@ class FrankaPlanRunner { momap::log()->debug("After LCM thread join"); return return_value; } -private: +private: int RunFranka() { //$ attempt connection to robot and read current mode @@ -353,6 +368,9 @@ class FrankaPlanRunner { setDefaultBehavior(robot); robot_alive_ = true; + // Load the kinematics and dynamics model. + franka::Model model = robot.loadModel(); + std::cout << "Ready." << std::endl; PublishTriggerToChannel(get_current_utime(), lcm_driver_status_channel_, true); @@ -380,13 +398,206 @@ class FrankaPlanRunner { return this->FrankaPlanRunner::JointPositionCallback(robot_state, period); }; + // error + std::array error_accumulator = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + + // Cartesian Compliance parameters + const double translational_stiffness{150.0}; + const double rotational_stiffness{10.0}; + Eigen::MatrixXd stiffness(6, 6), damping(6, 6); + stiffness.setZero(); + stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3); + stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3); + damping.setZero(); + damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * + Eigen::MatrixXd::Identity(3, 3); + damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) * + Eigen::MatrixXd::Identity(3, 3); + + // define callback for the torque control loop + std::function + impedance_control_callback = [&](const franka::RobotState& robot_state, + franka::Duration /*duration*/) -> franka::Torques { + // get state variables + std::array coriolis_array = model.coriolis(robot_state); + std::array jacobian_array = + model.zeroJacobian(franka::Frame::kEndEffector, robot_state); + + // convert to Eigen + Eigen::Map > coriolis(coriolis_array.data()); + Eigen::Map > jacobian(jacobian_array.data()); + Eigen::Map > q(robot_state.q.data()); + Eigen::Map > dq(robot_state.dq.data()); + + + + + // get desired position: + Eigen::Vector3d position_d; // (initial_transform.translation()); + Eigen::Quaterniond orientation_d; //(initial_transform.linear()); + Eigen::VectorXd desired_conf; + std::vector desired_conf_vec; + desired_conf_vec.assign(std::begin(robot_state.q_d), std::end(robot_state.q_d)) ; + desired_conf = du::v_to_e( desired_conf_vec ); + std::string tool_frame = "disher_2oz_tip"; + dracula->GetCS()->GetFK(tool_frame, desired_conf, position_d, orientation_d); + + // get actual position: + Eigen::Vector3d position; // (transform.translation()); + Eigen::Quaterniond orientation; //(transform.linear()); + Eigen::VectorXd actual_conf; + std::vector actual_conf_vec; + actual_conf_vec.assign(std::begin(robot_state.q), std::end(robot_state.q)) ; + actual_conf = du::v_to_e( actual_conf_vec ); + dracula->GetCS()->GetFK(tool_frame, actual_conf, position, orientation); + // get transform + + Eigen::Matrix4d t_mat = dracula->GetCS()->solveForwardKin(actual_conf, tool_frame, parameters::kBaseLinkWorldFrame); + Eigen::Affine3d transform(t_mat); + // Eigen::Vector3d position(transform.translation()); + // Eigen::Quaterniond orientation(transform.linear()); + + // compute error to desired equilibrium pose + // position error + Eigen::Matrix error; + error.head(3) << position - position_d; + + // orientation error + // "difference" quaternion + if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) { + orientation.coeffs() << -orientation.coeffs(); + } + // "difference" quaternion + Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d); + error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); + // Transform to base frame + error.tail(3) << -transform.linear() * error.tail(3); + + // compute control + Eigen::VectorXd tau_task(7), tau_d(7); + + // Spring damper system with damping ratio=1 + const drake::Isometry3 X_7E = + drake::Translation3(drake::Vector3(0.0, 0, 0)) * + drake::AngleAxis(0.0, drake::Vector3::UnitZ()); + const int num_velocities{dracula->GetCS()->GetRigidBodyTreeRef().get_num_velocities()}; + drake::VectorX v = drake::VectorX::Zero(num_velocities); + KinematicsCache cache = dracula->GetCS()->GetRigidBodyTreeRef().doKinematics(actual_conf, v); + RigidBodyFrame frame_E("frame_E", dracula->GetCS()->GetRigidBodyTreeRef().FindBody(tool_frame), X_7E); + Eigen::MatrixXd J = dracula->GetCS()->GetRigidBodyTreeRef().CalcFrameSpatialVelocityJacobianInWorldFrame(cache, frame_E); + //Calculate vector difference between current and target + Eigen::Vector3d difference_xyz = position - position_d; + Eigen::AngleAxis delta_angle_axis = + Eigen::AngleAxis(orientation.inverse() * orientation_d); + Eigen::Vector3d delta_orientation = delta_angle_axis.axis()*delta_angle_axis.angle(); + Eigen::VectorXd difference(6); // angle set config + difference << difference_xyz, delta_orientation; + + tau_task << J.transpose() * (-stiffness * difference - damping * (J * dq)); + // tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq)); + tau_d << tau_task + coriolis; + + std::array tau_d_array{}; + Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; + + std::vector tau_m_vec{0,0,0,0,0,0,0}; + std::vector tau_cmd_vec{0,0,0,0,0,0,0}; + // std::vector tau_J_franka{0,0,0,0,0,0,0}; + // std::vector err_accum_vec{0,0,0,0,0,0,0}; + tau_m_vec.assign(std::begin(robot_state.tau_J), std::end(robot_state.tau_J)) ; + tau_cmd_vec.assign(std::begin(robot_state.tau_J_d), std::end(robot_state.tau_J_d)) ; + // tau_J_franka.assign(std::begin(tau_d_rate_limited), std::end(tau_d_rate_limited)) ; + // err_accum_vec.assign(std::begin(error_accumulator), std::end(error_accumulator)) ; + momap::log()->info("tau_meas: {}", dru::v_to_e(tau_m_vec).transpose()); + momap::log()->info("tau_cmd: {}", dru::v_to_e(tau_cmd_vec).transpose()); + // momap::log()->info("error_accumulator: {}", dru::v_to_e(err_accum_vec).transpose()); + + + return tau_d_array; + }; + + /* + + // Set gains for the joint impedance control. + // Stiffness + const std::array k_gains = {{700.0, 700.0, 700.0, 700.0, 290.0, 180.0, 70.0}}; + // Damping + const std::array d_gains = {{50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}}; + // integral + const std::array i_gains = {{5.0, 5.0, 5.0, 5.0, 3.0, 2.5, 1.5}}; + + // Define callback for the joint torque control loop. + + std::function + impedance_control_callback = + [&model, &error_accumulator, k_gains, d_gains, i_gains]( + const franka::RobotState& state, franka::Duration ) -> franka::Torques { + + // Read current coriolis terms from model. + std::array coriolis = model.coriolis(state); + + // Compute torque command from joint impedance control law. + // Note: The answer to our Cartesian pose inverse kinematics is always in state.q_d with one + // time step delay. + std::array tau_d_calculated; + for (size_t i = 0; i < 7; i++) { + tau_d_calculated[i] = + k_gains[i] * (state.q_d[i] - state.q[i]) - d_gains[i] * state.dq[i] + i_gains[i] * error_accumulator[i] + coriolis[i]; + error_accumulator[i] += (state.q_d[i] - state.q[i]); + } + + // The following line is only necessary for printing the rate limited torque. As we activated + // rate limiting for the control loop (activated by default), the torque would anyway be + // adjusted! + std::array tau_d_rate_limited = + franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated, state.tau_J_d); + + std::vector tau_m_vec{0,0,0,0,0,0,0}; + std::vector tau_cmd_vec{0,0,0,0,0,0,0}; + std::vector tau_J_franka{0,0,0,0,0,0,0}; + std::vector err_accum_vec{0,0,0,0,0,0,0}; + tau_m_vec.assign(std::begin(state.tau_J), std::end(state.tau_J)) ; + tau_cmd_vec.assign(std::begin(state.tau_J_d), std::end(state.tau_J_d)) ; + tau_J_franka.assign(std::begin(tau_d_rate_limited), std::end(tau_d_rate_limited)) ; + err_accum_vec.assign(std::begin(error_accumulator), std::end(error_accumulator)) ; + momap::log()->info("tau_meas: {}", dru::v_to_e(tau_m_vec).transpose()); + momap::log()->info("tau_cmd: {}", dru::v_to_e(tau_cmd_vec).transpose()); + momap::log()->info("error_accumulator: {}", dru::v_to_e(err_accum_vec).transpose()); + + // Send torque command. + return tau_d_rate_limited; + }; + */ + + //callback for inverseDynamics - i think all this code should be refactored at some point... + std::function + inverse_dynamics_control_callback = [&, this]( + const franka::RobotState& robot_state, franka::Duration period) -> franka::Torques { + return this->FrankaPlanRunner::InverseDynamicsControlCallback(robot_state, period); + }; + + //for inverse dynamics + run_inverse_dynamics_ = false; // TODO : move this somewhere else in the class? + if(run_inverse_dynamics_){ + MultibodySetUp(mb_plant_, mb_plant_context_, "/src/drake-franka-driver/tests/data/franka_test_with_mass.urdf" ); + // std::cout << "done multibody setup" << '\n'; + // ref_vd_ = plan_.plan->derivative(2); + } + //$ main control loop while(true) { // std::cout << "top of loop: Executing motion." << std::endl; try { if (plan_.has_data && !paused_) { //$ prevent the plan from being started if robot is paused_ - robot.control(joint_position_callback); //impedance_control_callback + if(run_inverse_dynamics_) { + robot.control(inverse_dynamics_control_callback); + } + else{ + error_accumulator = {{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; + robot.control(impedance_control_callback, joint_position_callback); + // robot.control(joint_position_callback); + } } else { // publish robot_status // TODO: add a timer to be closer to 200 Hz. @@ -454,12 +665,12 @@ class FrankaPlanRunner { ConvertToArray(next_conf_vec, robot_state.q_d); ConvertToArray(vel, robot_state.dq); - + franka::JointPositions cmd_pos = JointPositionCallback(robot_state, period); prev_conf = next_conf.replicate(1,1); - next_conf = du::v_to_e(ConvertToVector(cmd_pos.q)); + next_conf = du::v_to_e(ConvertToVector(cmd_pos.q)); dracula->GetViz()->displayState(next_conf); next_conf_vec = du::e_to_v(next_conf); @@ -470,12 +681,12 @@ class FrankaPlanRunner { } milliseconds current_ms = duration_cast< milliseconds >(system_clock::now().time_since_epoch()); - int64_t delta_ms = int64_t( (current_ms - last_ms).count() ); + int64_t delta_ms = int64_t( (current_ms - last_ms).count() ); period = franka::Duration(delta_ms); last_ms = current_ms; } - return 0; + return 0; }; double StopPeriod(double period) { @@ -585,8 +796,8 @@ class FrankaPlanRunner { std::array current_cmd = robot_state.q_d; // set to actual, not desired std::array current_conf = robot_state.q_d; // set to actual, not desired desired_next = du::v_to_e( ConvertToVector(current_cmd) ); - - double error = DBL_MAX; + + double error = DBL_MAX; // const double cur_traj_time_s = static_cast(cur_time_us_ - start_time_us_) / 1e6; if (plan_.plan) { @@ -606,7 +817,7 @@ class FrankaPlanRunner { Eigen::VectorXd output_end = starting_q_eigen + delta_end; Eigen::VectorXd current_conf_eigen = du::v_to_e( ConvertToVector(current_conf) ); // error = ( du::v_to_e( ConvertToVector(current_conf) ) - plan_.plan->value(plan_.plan->end_time()) ).norm(); - error = ( current_conf_eigen - output_end ).norm(); + error = ( current_conf_eigen - output_end ).maxCoeff(); // momap::log()->warn("starting franka q = {}", du::v_to_e( ConvertToVector(starting_franka_q_) ).transpose()); // momap::log()->warn("starting_q_eigen = {}", starting_q_eigen.transpose()); @@ -629,7 +840,7 @@ class FrankaPlanRunner { // output = q_goal; if (franka_time_ > plan_.plan->end_time()) { - if (error < 0.007) { // TODO: replace with non arbitrary number + if (error < 0.005) { // TODO: replace with non arbitrary number franka::JointPositions ret_val = current_conf; std::cout << std::endl << "Finished motion, exiting controller" << std::endl; plan_.plan.release(); @@ -678,7 +889,7 @@ class FrankaPlanRunner { // Try to lock data to avoid read write collisions. if (robot_data_.mutex.try_lock()) { if (robot_data_.has_data) { - lcmt_iiwa_status franka_status = ConvertToLcmStatus(robot_data_.robot_state); + lcmt_iiwa_status franka_status = ConvertToLcmStatus(robot_data_.robot_state); // publish data over lcm lcm_.publish(p.lcm_status_channel, &franka_status); robot_data_.has_data = false; @@ -765,7 +976,8 @@ class FrankaPlanRunner { { if ( ! du::EpsEq(commanded_start(joint), robot_data_.robot_state.q[joint], p.kMediumJointDistance)) { - momap::log()->info("Discarding plan, mismatched start position."); + momap::log()->info("Discarding plan, mismatched start position {} vs robot_q {}.", + commanded_start(joint), robot_data_.robot_state.q[joint] ); plan_.has_data = false; plan_.plan.release(); plan_.mutex.unlock(); @@ -823,6 +1035,25 @@ class FrankaPlanRunner { } }; + /// + /// creates multibody plant and its context + /// param : mb_plant_ : is member of franka_plan_runner class, multibodyplant for calcinvdyn + /// param : mb_plant_context : is member of franka_plan_runner class, multibodyplant context + /// param : urdf_path : urdf to create multibodyplant + /// + void MultibodySetUp( drake::multibody::MultibodyPlant &mb_plant_ + , std::unique_ptr> &mb_plant_context_ + , const std::string urdf_path); + + /// + /// callback for inverse dynamics control + /// TODO :make kp,ki,kd part of the yaml + /// TO NOTE: when testing, it seemed that joint 0 was much more reactive than all the other joints + /// + franka::Torques InverseDynamicsControlCallback(const franka::RobotState& robot_state, franka::Duration period); + + + void Pause() { momap::log()->info("Pausing plan."); paused_ = false; @@ -846,4 +1077,4 @@ class FrankaPlanRunner { } // robot_plan_runner } // drake -#endif +#endif diff --git a/src/franka_plan_runner.cc b/src/franka_plan_runner.cc index 9075b49..2de9257 100644 --- a/src/franka_plan_runner.cc +++ b/src/franka_plan_runner.cc @@ -11,6 +11,13 @@ /// current plan and wait until a new plan is received. #include "franka_plan_runner.h" +#include +#include "drake/multibody/parsers/urdf_parser.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/multibody/plant/multibody_plant.h" + + +using drake::multibody::Parser; namespace drake { namespace franka_driver { @@ -21,9 +28,189 @@ int do_main(std::string param_yaml="franka_test.yaml") { momap::log()->info("Loading parameters: {}", param_yaml); parameters::Parameters params = parameters::loadYamlParameters(param_yaml, verbose); FrankaPlanRunner frankaPlanRunner(params); - return frankaPlanRunner.Run(); + return frankaPlanRunner.Run(); +} + +void FrankaPlanRunner::MultibodySetUp(drake::multibody::MultibodyPlant &mb_plant_ + , std::unique_ptr> &mb_plant_context_ + , const std::string urdf_path){ + Parser parser(&mb_plant_); + parser.AddModelFromFile(urdf_path, "mb_plant_"); + mb_plant_.mutable_gravity_field().set_gravity_vector( -9.8* drake::Vector3::UnitZ()); + mb_plant_.Finalize(); + mb_plant_context_ = mb_plant_.CreateDefaultContext(); } +franka::Torques FrankaPlanRunner::InverseDynamicsControlCallback(const franka::RobotState& robot_state, franka::Duration period){ + franka::Torques output = robot_state.tau_J; // TODO: initialize to something else + momap::log()->debug("entering callback"); + if ( plan_.mutex.try_lock() ) { + // we got the lock, so try and do stuff. + // momap::log()->info("got the lock!"); + // FrankaPlanRunner::check_franka_pause(); + + if (pausing_) { + if (target_stop_time_ == 0) { //if target_stop_time_ not set, set target_stop_time_ + std::array vel = robot_state.dq; + float temp_target_stop_time_ = 0; + for (int i = 0; i < 7; i++) { + float stop_time = fabs(vel[i] / (this->max_accels_[i])); //sets target stop_time in plan as max(vel_i/max_accel_i), where i is each joint. real world stop time ~ 2x stop_time in plan + if (stop_time > temp_target_stop_time_) { + temp_target_stop_time_ = stop_time; + } + } + target_stop_time_ = temp_target_stop_time_ / STOP_SCALE; + momap::log()->debug("TARGET: {}", target_stop_time_); + } + + double new_stop = StopPeriod(period.toSec()); + franka_time_ += new_stop; + momap::log()->debug("STOP PERIOD: {}", new_stop); + timestep_++; + + if (new_stop >= period.toSec() * p.stop_epsilon) { // robot counts as "stopped" when new_stop is less than a fraction of period + this->stop_duration_++; + } + else if (stop_margin_counter_ <= p.stop_margin) { // margin period after pause before robot is allowed to continue + stop_margin_counter_ += period.toSec(); + } + else{ + paused_ = true; + QueuedCmd(); + } + + + } else if (unpausing_) { //robot is unpausing_ + if (timestep_ >= 0) { //if robot has reached full speed again + unpausing_ = false; + QueuedCmd(); + } + double new_stop = StopPeriod(period.toSec()); + franka_time_ += new_stop; + momap::log()->debug("CONTINUE PERIOD: {}", new_stop); + timestep_++; + + } + else { + franka_time_ += period.toSec(); + } + + if (plan_.plan && plan_number_ != cur_plan_number) { + momap::log()->info("Starting new plan at {} s.", franka_time_); + start_time_us_ = cur_time_us_; // implies that we should have call motion finished + cur_plan_number = plan_number_; + starting_conf_ = plan_.plan->value(0.0); + starting_franka_q_ = robot_state.q_d; + momap::log()->warn("starting franka q = {}", du::v_to_e( ConvertToVector(starting_franka_q_) ).transpose()); + momap::log()->warn("difference between where we are and where we think = {}", + ( du::v_to_e( ConvertToVector(starting_franka_q_) ) - starting_conf_ ).norm() ); + } + + if (robot_data_.mutex.try_lock()) { + robot_data_.has_data = true; + robot_data_.robot_state = robot_state; + robot_data_.mutex.unlock(); + } + + if(plan_.plan){//below is inverseDynamics code + const Eigen::VectorXd kp = Eigen::VectorXd::Ones(kNumJoints)*10; + const Eigen::VectorXd ki = Eigen::VectorXd::Ones(kNumJoints)*10; + const Eigen::VectorXd kd = Eigen::VectorXd::Ones(kNumJoints)*10; + + if(franka_time_ == 0) integral_error = Eigen::VectorXd::Zero(kNumJoints); // initialize integral error to 0 at start time + + Eigen::VectorXd desired_vd = Eigen::VectorXd::Zero(kNumJoints); + // Eigen::Map > pos_desired(robot_state.q_d.data()); + Eigen::Map > pos_actual(robot_state.q.data()); + + //Robert : for testing can maintain one position + if(runonce){ + pos_start = pos_actual; + momap::log()->info("set pos_start to = {}", pos_start.transpose()); + runonce = false; + } + Eigen::Matrix pos_desired = pos_start; + + integral_error += pos_desired - pos_actual; + Eigen::Map > vel_desired(robot_state.dq_d.data()); + Eigen::Map > vel_actual(robot_state.dq.data()); + // auto t1 = std::chrono::system_clock::now(); // for timing purposes + // TODO : FIX SEGFAULT FOR REF_VD INITIALIZE/INSTANTIATE + // TODO : need ref_vd because doing the derivative of polynomial in realtime to get ref acceleration is too slow and takes 2.3 ms + // desired_vd = ref_vd_.value(franka_time) + + desired_vd = kp.cwiseProduct(pos_desired - pos_actual)+ kd.cwiseProduct(vel_desired - vel_actual) + ki.cwiseProduct(integral_error) ; + // for gravity compensation, desired_vd should be zero + + Eigen::VectorXd tau = Eigen::VectorXd::Zero(kNumJoints); + Eigen::VectorXd current_state(14); + current_state << pos_actual, vel_actual; + mb_plant_.SetPositionsAndVelocities(mb_plant_context_.get(), current_state); + + multibody::MultibodyForces external_forces(mb_plant_); + // Isabelle's old version : account for all external forces ( including gravity ) + // Eigen::Map > tau_cmd(robot_state.tau_J_d.data()); // convert std::array to eigen vector + // Eigen::Map > tau_measured(robot_state.tau_J.data()); // the signs are flipped on these because the link side is negative sign of motor side + // external_forces.mutable_generalized_forces() = tau_cmd - tau_measured; // external_forces = external torques we sense on the robot + + + // Robert;s version : only account for gravity + mb_plant_.CalcForceElementsContribution( *mb_plant_context_, &external_forces); //takes care of reading gravity + // momap::log()->info("gravity = {}", force_element_contrib.transpose()); + + // potentially useful methods : get external force (not including gravity) from robot sensors + // Eigen::Map > tau_ext(robot_state.tau_ext_hat_filtered.data()); + // external_forces.mutable_generalized_forces() = tau_ext; + auto tau_id = mb_plant_.CalcInverseDynamics(*mb_plant_context_, desired_vd, external_forces); //calculates the M(q) + C(q) - tau_ap + // auto t4 = std::chrono::system_clock::now(); + auto tau_gravity_only = mb_plant_.CalcGravityGeneralizedForces(*mb_plant_context_); + + std::vector tau_m_vec{0,0,0,0,0,0,0}; + std::vector tau_cmd_vec{0,0,0,0,0,0,0}; + tau_m_vec.assign(std::begin(robot_state.tau_J), std::end(robot_state.tau_J)) ; + tau_cmd_vec.assign(std::begin(robot_state.tau_J_d), std::end(robot_state.tau_J_d)) ; + momap::log()->info("tau_meas: {}", dru::v_to_e(tau_m_vec).transpose()); + momap::log()->info("tau_cmd: {}", dru::v_to_e(tau_cmd_vec).transpose()); + momap::log()->info("gravity: {}", tau_gravity_only.transpose()); + momap::log()->info("tau_ID: {}", tau_id.transpose()); + // tau = -1*tau_gravity_only; + tau = tau_id; + + //calling franka::limitrate is unnecessary because robot.control has limit_rate= default true? + + output = {{ tau[0], tau[1], + tau[2], tau[3], + tau[4], tau[5], + tau[6] }}; + // throw std::exception(); // stops robot before actually sending output + // where we are now + std::array current_q = robot_state.q; + Eigen::VectorXd current_conf = du::v_to_e( ConvertToVector(current_q)); + // where we want to go + Eigen::VectorXd desired_end_conf = plan_.plan->value(plan_.plan->end_time()); + // norm distance from desired_end_conf + double dist_from_end = (current_conf - desired_end_conf).norm(); + if (franka_time_ > plan_.plan->end_time()) { + if (dist_from_end < 0.007) { + plan_.plan.release(); + plan_.has_data = false; + // plan_.utime = -1; + plan_.mutex.unlock(); + + PublishTriggerToChannel(plan_.utime, p.lcm_plan_complete_channel); + return franka::MotionFinished(output); + } else { + momap::log()->debug("Plan running overtime and not converged, dist: {}", + dist_from_end); + } + + } + } + plan_.mutex.unlock(); + momap::log()->debug("returning at end of callback"); + return output; + } + return franka::MotionFinished(output); +} } // namespace robot_plan_runner } // namespace drake @@ -42,3 +229,5 @@ int main(int argc, char** argv) { return drake::franka_driver::do_main(param_yaml); } } + + diff --git a/tests/data/franka_test_with_mass.urdf b/tests/data/franka_test_with_mass.urdf new file mode 100644 index 0000000..0b17034 --- /dev/null +++ b/tests/data/franka_test_with_mass.urdf @@ -0,0 +1,389 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/data/old_franka_test_with_mass_no_joint_limits.urdf b/tests/data/old_franka_test_with_mass_no_joint_limits.urdf new file mode 100644 index 0000000..bb7f6d3 --- /dev/null +++ b/tests/data/old_franka_test_with_mass_no_joint_limits.urdf @@ -0,0 +1,474 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + +