// -------------------------------------------------------------------------------------
KDL::Jacobian JacoIKSolver::desiredJacobian(Eigen::MatrixXd qd)
{
KDL::JntArray joint_in;
KDL::Jacobian jac;
joint_in.resize(6);
jac.resize(chain_.getNrOfJoints());
qd = DTR*qd;
for (int i = 0; i < 6; i++){
joint_in(i) = qd(0,i);
}
jnt_to_jac_solver_->JntToJac(joint_in, jac);
return jac;
}
KDL::Jacobian JacoIKSolver::jointToJacobian(JacoAngles JAngle)
{
KDL::JntArray joint_in;
KDL::Jacobian jac;
joint_in.resize(6);
jac.resize(chain_.getNrOfJoints());
joint_in(0) = ( JAngle.Actuator1 - 180.0 ) * DTR;
joint_in(1) = ( JAngle.Actuator2 - 270.0 ) * DTR;
joint_in(2) = ( JAngle.Actuator3 - 90.0 ) * DTR;
joint_in(3) = ( JAngle.Actuator4 - 180.0 ) * DTR;
joint_in(4) = ( JAngle.Actuator5 - 180.0 ) * DTR;
joint_in(5) = ( JAngle.Actuator6 - 270.0 ) * DTR;
jnt_to_jac_solver_->JntToJac(joint_in, jac);
return jac;
}
VirtualForcePublisher()
{
ros::NodeHandle n_tilde("~");
ros::NodeHandle n;
// subscribe to joint state
joint_state_sub_ = n.subscribe("joint_states", 1, &VirtualForcePublisher::callbackJointState, this);
wrench_stamped_pub_ = n.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
// set publish frequency
double publish_freq;
n_tilde.param("publish_frequency", publish_freq, 50.0);
publish_interval_ = ros::Duration(1.0/std::max(publish_freq,1.0));
//set time constant of low pass filter
n_tilde.param("time_constant", t_const_, 0.3);
// set root and tip
n_tilde.param("root", root, std::string("torso_lift_link"));
n_tilde.param("tip", tip, std::string("l_gripper_tool_frame"));
// load robot model
urdf::Model robot_model;
robot_model.initParam("robot_description");
KDL::Tree kdl_tree;
kdl_parser::treeFromUrdfModel(robot_model, kdl_tree);
kdl_tree.getChain(root, tip, chain_);
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
jnt_pos_.resize(chain_.getNrOfJoints());
jacobian_.resize(chain_.getNrOfJoints());
for (size_t i=0; i<chain_.getNrOfSegments(); i++) {
if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) {
std::cerr << "kdl_chain(" << i << ") " << chain_.getSegment(i).getJoint().getName().c_str() << std::endl;
}
}
}
int SNS_IK::CartToJntVel(const KDL::JntArray& q_in, const KDL::Twist& v_in,
const KDL::JntArray& q_bias,
const std::vector<std::string>& biasNames,
const KDL::JntArray& q_vel_bias,
KDL::JntArray& qdot_out)
{
if (!m_initialized) {
ROS_ERROR("SNS_IK was not properly initialized with a valid chain or limits.");
return -1;
}
KDL::Jacobian jacobian;
jacobian.resize(q_in.rows());
if (m_jacobianSolver->JntToJac(q_in, jacobian) < 0)
{
std::cout << "JntToJac failed" << std::endl;
return -1;
}
std::vector<Task> sot;
Task task;
task.jacobian = jacobian.data;
task.desired = VectorD::Zero(6);
// twistEigenToKDL
for(size_t i = 0; i < 6; i++)
task.desired(i) = v_in[i];
sot.push_back(task);
// Calculate the nullspace goal as a configuration-space task.
// Creates a task Jacobian which maps the provided nullspace joints to
// the full joint state.
if (q_bias.rows()) {
Task task2;
std::vector<int> indicies;
if (!nullspaceBiasTask(q_bias, biasNames, &(task2.jacobian), &indicies)) {
ROS_ERROR("Could not create nullspace bias task");
return -1;
}
task2.desired = VectorD::Zero(q_bias.rows());
for (size_t ii = 0; ii < q_bias.rows(); ++ii) {
// This calculates a "nullspace velocity".
// There is an arbitrary scale factor which will be set by the max scale factor.
task2.desired(ii) = m_nullspaceGain * (q_bias(ii) - q_in(indicies[ii])) / m_loopPeriod;
// TODO: may want to limit the NS velocity to 70-90% of max joint velocity
}
sot.push_back(task2);
}
// Bias the joint velocities
// If the bias is the previous joint velocities, this is velocity damping
if(q_vel_bias.rows() == q_in.rows()) {
Task task2;
task2.jacobian = MatrixD::Identity(q_vel_bias.rows(), q_vel_bias.rows());
task2.desired = VectorD::Zero(q_vel_bias.rows());
for (size_t ii = 0; ii < q_vel_bias.rows(); ++ii) {
task2.desired(ii) = q_vel_bias(ii);
}
sot.push_back(task2);
}
return m_ik_vel_solver->getJointVelocity(&qdot_out.data, sot, q_in.data);
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "kinematics_publisher");
ros::NodeHandle nh;
ros::Publisher joint_publisher;
ros::Publisher frame_publisher;
joint_publisher = nh.advertise<sensor_msgs::JointState>("kinematics/joint_states", 1, true);
frame_publisher = nh.advertise<geometry_msgs::PoseStamped>("/frame_result", 1, true);
std::vector<std::string> names;
names.resize(2);
names.at(0) = "base_to_body";
names.at(1) = "body_to_head";
sensor_msgs::JointState my_joints;
my_joints.name = names;
my_joints.position.resize(2);
my_joints.position.at(0) = 0.2;
my_joints.position.at(1) = 0,2;
joint_publisher.publish(my_joints);
ros::spinOnce();
std::cin.get();
bool exit_value;
KDL::Tree my_tree;
urdf::Model my_model;
if (!kdl_parser::treeFromParam("/robot_description", my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
std::cout << "Number of links of the urdf:" << my_tree.getNrOfSegments() << std::endl;
std::cout << "Number of Joints of the urdf:" << my_tree.getNrOfJoints() << std::endl;
KDL::Chain my_chain;
my_tree.getChain("world", "webcam", my_chain);
std::cout << "Number of links of the CHAIN:" << my_chain.getNrOfSegments() << std::endl;
std::cout << "Number of Joints of the CHAIN:" << my_chain.getNrOfJoints() << std::endl;
KDL::ChainFkSolverPos_recursive fksolver(my_chain);
KDL::JntArray q(my_chain.getNrOfJoints());
q(0) = my_joints.position.at(0);
q(1) = my_joints.position.at(1);
KDL::Frame webcam;
fksolver.JntToCart(q,webcam);
geometry_msgs::Pose webcam_pose;
tf::poseKDLToMsg(webcam, webcam_pose);
geometry_msgs::PoseStamped webcam_stamped;
webcam_stamped.pose = webcam_pose;
webcam_stamped.header.frame_id = "world";
webcam_stamped.header.stamp = ros::Time::now();
frame_publisher.publish(webcam_stamped);
ros::spinOnce();
KDL::ChainJntToJacSolver jacobian_solver(my_chain);
KDL::Jacobian J;
J.resize(2);
jacobian_solver.JntToJac(q, J);
std::cout << J.data << std::endl;
webcam.M.DoRotY(-20*KDL::deg2rad);
tf::poseKDLToMsg(webcam, webcam_pose);
webcam_stamped.pose = webcam_pose;
webcam_stamped.header.stamp = ros::Time::now();
frame_publisher.publish(webcam_stamped);
ros::spinOnce();
std::cin.get();
KDL::ChainIkSolverVel_pinv ik_solver_vel(my_chain);
KDL::ChainIkSolverPos_NR ik_solver_pos(my_chain,fksolver,ik_solver_vel);
KDL::JntArray q_sol(2);
ik_solver_pos.CartToJnt(q, webcam,q_sol);
my_joints.position.at(0) = q_sol(0);
my_joints.position.at(1) = q_sol(1);
joint_publisher.publish(my_joints);
ros::spinOnce();
std::cin.get();
KDL::Vector g (0.0,0.0,-9.81);
KDL::ChainDynParam my_chain_params(my_chain, g);
KDL::ChainIdSolver_RNE id_solver(my_chain, g);
KDL::JntArray G(2);
my_chain_params.JntToGravity(q_sol,G);
KDL::JntSpaceInertiaMatrix H(2);
my_chain_params.JntToMass(q_sol, H);
KDL::JntArray C(2);
KDL::JntArray q_dot(2);
q_dot(0) = 0.02;
q_dot(1) = 0.04;
my_chain_params.JntToCoriolis(q_sol,q_dot, C);
std::cout << "g(q): " <<G.data << std::endl;
std::cout << "M(q): " <<H.data << std::endl;
std::cout << "C(q,q_dot): " <<C.data << std::endl;
KDL::JntArray q_dotdot(2);
q_dotdot(0) = -0.02;
q_dotdot(1) = 0.02;
KDL::Wrenches f(4);
KDL::JntArray tau(2);
id_solver.CartToJnt(q_sol, q_dot, q_dotdot, f, tau);
std::cout << "Tau: " <<tau.data << std::endl;
std::cin.get();
moveit::planning_interface::MoveGroup my_group("webcam_robot");
my_group.setNamedTarget("look_left");
my_group.move();
return false;
}
