diff --git a/include/franka_plan_runner.h b/include/franka_plan_runner.h index a07dade..0599212 100755 --- a/include/franka_plan_runner.h +++ b/include/franka_plan_runner.h @@ -88,7 +88,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; @@ -143,7 +143,7 @@ static void AssignToLcmStatus(franka::RobotState &robot_state, lcmt_iiwa_status } lcmt_iiwa_status ConvertToLcmStatus(franka::RobotState &robot_state){ - lcmt_iiwa_status robot_status{}; + lcmt_iiwa_status robot_status{}; int num_joints_ = robot_state.q.size(); struct timeval tv; gettimeofday(&tv, NULL); @@ -159,7 +159,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_){ @@ -186,7 +186,7 @@ int64_t get_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}; @@ -198,7 +198,7 @@ class FrankaPlanRunner { int64_t cur_time_us{}; int64_t start_time_us{}; RobotPiecewisePolynomial plan_; - RobotData robot_data_{}; + RobotData robot_data_{}; PPType piecewise_polynomial; std::thread lcm_publish_status_thread; std::thread lcm_handle_thread; @@ -213,21 +213,28 @@ class FrankaPlanRunner { std::atomic_bool pausing; std::atomic_bool paused; std::atomic_bool unpausing; - float STOP_MARGIN; + float STOP_MARGIN; float stop_margin_counter = 0; - int queued_cmd = 0; //0: None, 1: Pause, 2: Continue + int queued_cmd = 0; //0: None, 1: Pause, 2: Continue Eigen::VectorXd starting_conf; - std::array starting_franka_q; - - + std::array starting_franka_q; + // 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 // Set print rate for comparing commanded vs. measured torques. const double lcm_publish_rate = 200.0; //Hz double franka_time; Eigen::VectorXd max_accels; + Eigen::Matrix pos_start; + bool runonce = true; + public: FrankaPlanRunner(const parameters::Parameters params) : p(params), ip_addr_(params.robot_ip), plan_number_(0), @@ -235,7 +242,7 @@ class FrankaPlanRunner { lcm_.subscribe(p.lcm_plan_channel, &FrankaPlanRunner::HandlePlan, this); lcm_.subscribe(p.lcm_stop_channel, &FrankaPlanRunner::HandleStop, this); running_ = true; - franka_time = 0.0; + franka_time = 0.0; max_accels = params.robot_max_accelerations; STOP_EPSILON = params.stop_epsilon; @@ -248,17 +255,17 @@ class FrankaPlanRunner { cur_plan_number = plan_number_; cur_time_us = -1; start_time_us = -1; - sign_ = +1; + sign_ = +1; pausing = false; paused = false; unpausing = false; - starting_franka_q = {{0,0,0,0,0,0,0}}; + starting_franka_q = {{0,0,0,0,0,0,0}}; starting_conf = Eigen::VectorXd::Zero(kNumJoints); - plan_.has_data = false; + plan_.has_data = false; plan_.plan.release(); plan_.utime = -1; plan_.end_time_us = 0; @@ -267,9 +274,10 @@ 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() { // if (lcm_publish_status_thread.joinable()) { // lcm_publish_status_thread.join(); @@ -294,7 +302,7 @@ class FrankaPlanRunner { } return return_value; } -private: +private: int RunFranka() { // const double print_rate = 10.0; // struct { @@ -342,7 +350,7 @@ class FrankaPlanRunner { // Connect to robot. franka::Robot robot(ip_addr_); setDefaultBehavior(robot); - robot_alive_ = true; + robot_alive_ = true; // First move the robot to a suitabcurrent_desiredle joint configuration // std::array q_goal = {current_desired{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; // MotionGenerator motion_generator(0.5, q_goal); @@ -357,14 +365,14 @@ class FrankaPlanRunner { // Set additional parameters always before the control loop, NEVER in the control loop! // Set collision behavior. - bool we_care_about_safety = false; - if (we_care_about_safety) { + bool we_care_about_safety = true; + if (we_care_about_safety) { robot.setCollisionBehavior( {{40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0}}, {{40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0}}, {{40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0}}, {{40.0, 40.0, 36.0, 36.0, 32.0, 28.0, 24.0}}, {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}, {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}, {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}, {{40.0, 40.0, 40.0, 50.0, 50.0, 50.0}}); - } else { + } else { robot.setCollisionBehavior( {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, @@ -378,15 +386,34 @@ class FrankaPlanRunner { return this->FrankaPlanRunner::JointPositionCallback(robot_state, period); }; - + + 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_no_joint_limits.urdf" ); + // std::cout << "done multibody setup" << '\n'; + // ref_vd_ = plan_.plan->derivative(2); + } + while(true){ // std::cout << "top of loop: Executing motion." << std::endl; try { if (plan_.has_data) { - robot.control(joint_position_callback); //impedance_control_callback + if(run_inverse_dynamics_) { + robot.control(inverse_dynamics_control_callback); + } + else{ + robot.control(joint_position_callback); + } } else { // publish robot_status - // TODO: add a timer to be closer to 200 Hz. + // TODO: add a timer to be closer to 200 Hz. // std::cout << "only should be here when sitting.\n"; robot.read([this](const franka::RobotState& robot_state) { if (this->robot_data_.mutex.try_lock()) { @@ -399,31 +426,31 @@ class FrankaPlanRunner { return false; }); } - + } catch (const franka::ControlException& e) { std::cout << e.what() << std::endl; std::cout << "Running error recovery..." << std::endl; if (plan_.mutex.try_lock() ) { robot.automaticErrorRecovery(); - plan_.mutex.unlock(); + plan_.mutex.unlock(); } else { momap::log()->error("failed to get a mutex after an error. returning -99."); - return -99; + return -99; } - + } } } catch (const franka::Exception& ex) { running_ = false; momap::log()->error("drake::franka_driver::RunFranka Caught expection: {}", ex.what() ); - // return -99; // bad things happened. + // return -99; // bad things happened. } // if (print_thread.joinable()) { // print_thread.join(); // } - return 0; - + return 0; + }; int RunSim() { @@ -448,28 +475,28 @@ 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); std::vector prev_conf_vec = du::e_to_v(prev_conf); for(int i=0; i<7; i++){ - vel[i] = (next_conf_vec[i] - prev_conf_vec[i]) / (double) period.toSec(); + vel[i] = (next_conf_vec[i] - prev_conf_vec[i]) / (double) period.toSec(); } 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){ @@ -494,6 +521,57 @@ class FrankaPlanRunner { queued_cmd = 0; } + /// + /// function that is part of callbacks. purpose : to check time and pausing status + /// + // void 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; + // } + // } + // this->target_stop_time = temp_target_stop_time; + // 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() * STOP_EPSILON){ // robot counts as "stopped" when new_stop is less than a fraction of period + // this->stop_duration++; + // } + // else if(stop_margin_counter <= 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(); + // } + // } + franka::JointPositions JointPositionCallback( const franka::RobotState& robot_state , franka::Duration period ) { @@ -532,8 +610,8 @@ class FrankaPlanRunner { paused = true; QueuedCmd(); } - - + + } else if (unpausing) { //robot is unpausing if (timestep >= 0) { //if robot has reached full speed again unpausing = false; @@ -543,13 +621,13 @@ class FrankaPlanRunner { franka_time += new_stop; momap::log()->debug("CONTINUE PERIOD: {}", new_stop); timestep++; - + } else { franka_time += period.toSec(); } - cur_time_us = int64_t(franka_time * 1.0e6); + cur_time_us = int64_t(franka_time * 1.0e6); // TODO: remove the need for this check. who cares if it is a new plan? // TODO: make sure we've called motion finished and reset the timer? @@ -559,9 +637,9 @@ class FrankaPlanRunner { 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 = {}", + 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() ); } @@ -578,8 +656,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) { @@ -592,17 +670,17 @@ class FrankaPlanRunner { desired_next(j) = joint_limits(j,0); } } - Eigen::VectorXd delta = desired_next - starting_conf; - Eigen::VectorXd output_eigen = du::v_to_e( ConvertToVector(starting_franka_q) ) + delta; - Eigen::VectorXd delta_end = plan_.plan->value(plan_.plan->end_time()) - starting_conf; + Eigen::VectorXd delta = desired_next - starting_conf; + Eigen::VectorXd output_eigen = du::v_to_e( ConvertToVector(starting_franka_q) ) + delta; + Eigen::VectorXd delta_end = plan_.plan->value(plan_.plan->end_time()) - starting_conf; Eigen::VectorXd starting_q_eigen = du::v_to_e( ConvertToVector(starting_franka_q) ); - Eigen::VectorXd output_end = starting_q_eigen + delta_end; + 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(); - // 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()); + // 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()); // momap::log()->info("error: {}", error); // momap::log()->info("current_conf_eigen: {}", current_conf_eigen.transpose()); // momap::log()->info("output_end: {}", output_end.transpose()); @@ -619,8 +697,8 @@ class FrankaPlanRunner { output_eigen[6] }}; // std::array q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; - // output = q_goal; - + // output = q_goal; + if (franka_time > plan_.plan->end_time()) { if (error < 0.007) { // TODO: replace with non arbitrary number franka::JointPositions ret_val = current_conf; @@ -629,7 +707,7 @@ class FrankaPlanRunner { plan_.has_data = false; // plan_.utime = -1; plan_.mutex.unlock(); - + PublishUtimeToChannel(plan_.utime, p.lcm_plan_complete_channel); // return output; // plan_.mutex.unlock(); @@ -644,12 +722,12 @@ class FrankaPlanRunner { } else { // momap::log()->error("Inside JPC but plan_.plan != True"); } - + plan_.mutex.unlock(); return output; } - + // we couldn't get the lock, so probably need to return motion::finished() // plan_.mutex.unlock(); return franka::MotionFinished(output); @@ -667,11 +745,11 @@ class FrankaPlanRunner { // Sleep to achieve the desired print rate. std::this_thread::sleep_for( std::chrono::milliseconds(static_cast( 1000.0/lcm_publish_rate ))); - + // 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; @@ -688,7 +766,7 @@ class FrankaPlanRunner { dummy_status.utime = utime; lcm_.publish(lcm_channel, &dummy_status); } - + void HandlePlan(const ::lcm::ReceiveBuffer*, const std::string&, const robot_spline_t* rst) { momap::log()->info("New plan received."); @@ -702,9 +780,9 @@ class FrankaPlanRunner { std::this_thread::sleep_for( std::chrono::milliseconds(static_cast( 1.0 ))); } - + editing_plan = true; - + momap::log()->info("utime: {}", rst->utime); plan_.utime = rst->utime; //$ publish confirmation that plan was received with same utime @@ -732,7 +810,8 @@ class FrankaPlanRunner { { if (!du::EpsEq(commanded_start(joint),robot_data_.robot_state.q[joint], 0.05))//FIXME: non-arbitrary tolerance { - 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(); @@ -742,14 +821,14 @@ class FrankaPlanRunner { plan_.plan.release(); plan_.plan.reset(&piecewise_polynomial); - plan_.has_data = true; + plan_.has_data = true; ++plan_number_; - momap::log()->warn("Finished Handle Plan!"); + momap::log()->warn("Finished Handle Plan!"); editing_plan = false; plan_.mutex.unlock(); - + }; void HandleStop(const ::lcm::ReceiveBuffer*, const std::string&, @@ -784,13 +863,31 @@ 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); + + + }; } // robot_plan_runner } // drake -#endif +#endif diff --git a/src/franka_plan_runner.cc b/src/franka_plan_runner.cc index 9075b49..3f46a04 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,178 @@ 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_.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; + } + } + this->target_stop_time = temp_target_stop_time; + 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() * STOP_EPSILON){ // robot counts as "stopped" when new_stop is less than a fraction of period + this->stop_duration++; + } + else if(stop_margin_counter <= 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)*0; + const Eigen::VectorXd ki = Eigen::VectorXd::Ones(kNumJoints)*0; + const Eigen::VectorXd kd = Eigen::VectorXd::Ones(kNumJoints)*0; + + 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); + 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()->debug("gravity = {}", external_forces.generalized_forces().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; + tau = 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(); + // momap::log()->info("gravity calc v2= {}", mb_plant_.CalcGravityGeneralizedForces(*mb_plant_context_)); + // momap::log()->info("tau = {}", tau.transpose()); + + //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(); + + PublishUtimeToChannel(plan_.utime, p.lcm_plan_complete_channel); + return franka::MotionFinished(output); + } else { + momap::log()->info("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 +218,5 @@ int main(int argc, char** argv) { return drake::franka_driver::do_main(param_yaml); } } + + diff --git a/tests/data/franka_test_with_mass_no_joint_limits.urdf b/tests/data/franka_test_with_mass_no_joint_limits.urdf new file mode 100644 index 0000000..bb7f6d3 --- /dev/null +++ b/tests/data/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 + + + + + + + + + + + + + + + + + + + + + + + + + + +