qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Removed the teleop node

Browse Source
This commit is contained in:
Juancho
2023-06-07 10:58:44 +09:00
parent f5a35ec2bd
commit 01acfa3b30
1 changed files with 0 additions and 235 deletions
Download Patch File Download Diff File Expand all files Collapse all files

235
src/teleop_franka_node.cpp
View File

@@ -1,235 +0,0 @@
#include <ros/ros.h>
#include <sas_common/sas_common.h>
#include <sas_robot_kinematics/sas_robot_kinematics_provider.h>
#include <sas_robot_driver/sas_robot_driver_interface.h>
#include <iostream>
#include <Eigen/Dense>
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
//#include <dqrobotics/robots/FrankaEmikaPandaMDHRobot.h>
#include <dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h>
//#include <dqrobotics/solvers/DQ_SerialManipulatorDynamicsGaussPrincipleSolver.h>
#include <dqrobotics/solvers/DQ_QuadraticProgrammingSolver.h>
#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.h>
#include <thread>
#include <dqrobotics/DQ.h>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
#include <chrono>
#include <yaml-cpp/yaml.h>
#include <RobotConstraintsManager.h>
#include "franka_ros_interface.h"
#include <QuadraticProgramMotionGenerator.h>
std::string config_vfi_path = "/home/moonshot/Documents/git/franka_emika/real_franka_developments/real_franka_developments/cfg/vfi_constraints.yaml";
int main(int argc, char **argv)
{
ros::init(argc, argv, "teleop_franka_node");
auto nodehandle = ros::NodeHandle();
sas::RobotKinematicsProvider pose1_provider(nodehandle, "/arm1_kinematics");
//auto franka1_ros = FrankaRosInterface(nodehandle, 50, "/franka_1");
sas::RobotDriverInterface franka1_ros(nodehandle, "/franka_1");
//##########################################################################################
DQ_VrepInterface vi;
vi.connect("10.198.113.159", 19997,100,10);
std::cout << "Starting V-REP simulation..." << std::endl;
vi.start_simulation();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto franka_sim = FrankaEmikaPandaVrepRobot("Franka", std::make_shared<DQ_VrepInterface>(vi));
auto robot = franka_sim.kinematics();
DQ xoffset = (1 + E_*0.5*0.21*k_)*(cos((M_PI/4)/2)-k_*sin((M_PI/4)/2));
robot.set_effector(xoffset);
//###########################################################################################
auto manager = RobotConstraintsManager(std::make_shared<DQ_VrepInterface>(vi),
(std::static_pointer_cast<DQ_Kinematics>(std::make_shared<DQ_SerialManipulatorMDH>(robot))),
(std::static_pointer_cast<DQ_VrepRobot>(std::make_shared<FrankaEmikaPandaVrepRobot>(franka_sim))),
config_vfi_path,
RobotConstraintsManager::ORDER::FIRST_ORDER);
//##########################################################################################
//#######------------------------Controller------------------------------------------#######
DQ_QPOASESSolver solver;
DQ_ClassicQPController controller
(std::static_pointer_cast<DQ_Kinematics>(std::make_shared<DQ_SerialManipulatorMDH>(robot)),
std::static_pointer_cast<DQ_QuadraticProgrammingSolver>(std::make_shared<DQ_QPOASESSolver>(solver)));
controller.set_gain(20.0);
controller.set_damping(0.01);
controller.set_control_objective(DQ_robotics::Translation); //DistanceToPlane Translation
controller.set_stability_threshold(0.0001);
//##########################################################################################
//#########################################################################################
DQ xd;
DQ x;
MatrixXd A;
VectorXd b;
VectorXd u;
VectorXd q_u = VectorXd::Zero(7);
VectorXd q;
VectorXd q_dot_initial = VectorXd::Zero(7);
VectorXd q_dot = VectorXd::Zero(7);
while (ros::ok()) {
if (franka1_ros.is_enabled())
{
q = franka1_ros.get_joint_positions();
break;
}
ros::spinOnce();
}
q_u = q;
franka_sim.set_target_configuration_space_positions(q);
vi.set_object_pose("xd", robot.fkm(q));
/*
DQ_QPOASESSolver solver2;
MatrixXd H = MatrixXd::Identity(7, 7);
VectorXd f ;
VectorXd qvrep;
MatrixXd A_;
VectorXd b_;
for (int i=0;i<3000;i++)
{
qvrep = franka_sim.get_configuration_space_positions();
f = 2*0.1*(qvrep-q);
auto u_ = solver2.solve_quadratic_program(H, f, A_, b_, A_, b_);
franka_sim.set_target_configuration_space_velocities(u_);
std::cout<<"initialiazing..."<<std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
*/
std::cout<<"-------------------------"<<std::endl;
std::cout<<"Initial q: "<<std::endl;
std::cout<<q.transpose()<<std::endl;
std::cout<<"-------------------------"<<std::endl;
double T = 0.001;
try {
while(ros::ok())
{
if (q.size() == 7)
{
q = franka1_ros.get_joint_positions();
franka_sim.set_target_configuration_space_positions(q);
//q = franka_sim.get_configuration_space_positions();
x = robot.fkm(q);
vi.set_object_pose("effector", x);
if(not pose1_provider.is_enabled())
{
pose1_provider.send_pose(x);
}
pose1_provider.send_reference_frame(robot.get_reference_frame());
try{
xd = pose1_provider.get_desired_pose();
pose1_provider.send_pose(xd);
}
catch(const std::exception& e){
xd = vi.get_object_pose("xd");
std::cout<<"Nope"<<std::endl;
std::cout << e.what() << std::endl;
}
xd = vi.get_object_pose("xd");
std::tie(A, b) = manager.get_inequality_constraints(q);
controller.set_inequality_constraint(A,b);
//-----------------------------------------------------------
switch(controller.get_control_objective())
{
case ControlObjective::Distance:
{
VectorXd pdesired = vec4(translation(xd));
VectorXd distance(1);
distance(0) = pdesired.transpose()*pdesired;
u = controller.compute_setpoint_control_signal(q, pdesired.transpose()*pdesired);
break;
}
case ControlObjective::Translation:
{
u = controller.compute_setpoint_control_signal(q, vec4(xd.translation()));
break;
}
case ControlObjective::Pose:
{
u = controller.compute_setpoint_control_signal(q, vec8(xd));
break;
}
case ControlObjective::DistanceToPlane:
{
DQ plane_normal = xd.P()*k_*xd.P().conj();
DQ plane_point = xd.translation();
DQ xdesired_plane = (plane_normal + E_ * dot(plane_point, plane_normal));
controller.set_target_primitive(xdesired_plane);
u = controller.compute_setpoint_control_signal(q, VectorXd::Zero(1)); //DistanceToPlane
break;
}
default:
throw std::runtime_error("ControlObjective not suppported.");
break;
}
//q = q + T*u;
//std::cout<<"error: "<<std::endl;
//std::cout<<controller.get_last_error_signal().norm()<<std::endl;
//franka_sim.set_target_configuration_space_positions(franka1_ros.get_joint_positions());
//auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(u, T);
//franka_sim.set_target_configuration_space_velocities(new_u);
//franka1_ros.send_target_joint_positions(new_u);
//auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(u, T);
//franka1_ros.send_target_joint_positions(q);
franka1_ros.send_target_joint_velocities(u);
}
ros::spinOnce();
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << "Stopping V-REP simulation..." << std::endl;
vi.stop_simulation();
vi.disconnect();
return 0;
}
catch (const std::exception& e) {
vi.stop_simulation();
vi.disconnect();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << e.what() << std::endl;
return -1;
}
}