Esempio n. 1
0
// -------------------------------------------------------------------------------------
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;
}
Esempio n. 2
0
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;
}