void leatherman::printKDLChain(const KDL::Chain &c, std::string text)
{
ROS_INFO("[%s] # segments: %d # joints: %d", text.c_str(), c.getNrOfSegments(), c.getNrOfJoints());
double r,p,y,jnt_pos = 0;
for(size_t j = 0; j < c.getNrOfSegments(); ++j)
{
c.getSegment(j).pose(jnt_pos).M.GetRPY(r,p,y);
ROS_INFO("[%s] [%2d] segment: %21s xyz: %0.3f %0.3f %0.3f rpy: %0.3f %0.3f %0.3f",
text.c_str(), int(j),
c.getSegment(j).getName().c_str(),
c.getSegment(j).pose(jnt_pos).p.x(),
c.getSegment(j).pose(jnt_pos).p.y(),
c.getSegment(j).pose(jnt_pos).p.z(),
r,p,y);
c.getSegment(j).getJoint().pose(jnt_pos).M.GetRPY(r,p,y);
ROS_INFO("[%s] joint: %21s xyz: %0.3f %0.3f %0.3f rpy: %0.3f %0.3f %0.3f type: %s",
text.c_str(),
c.getSegment(j).getJoint().getName().c_str(),
c.getSegment(j).getJoint().pose(jnt_pos).p.x(),
c.getSegment(j).getJoint().pose(jnt_pos).p.y(),
c.getSegment(j).getJoint().pose(jnt_pos).p.z(),
r,p,y,
c.getSegment(j).getJoint().getTypeName().c_str());
}
}
int main()
{
KDLCollada kdlCollada;
vector <KDL::Chain> kinematicsModels;
const string filename = "puma.dae"; // loading collada kinematics model and converting it to kdl serial chain
if (!kdlCollada.load(COLLADA_MODELS_PATH + filename, kinematicsModels))
{
cout << "Failed to import " << filename;
return 0;
}
cout << "Imported " << kinematicsModels.size() << " kinematics chains" << endl;
for (unsigned int i = 0; i < kinematicsModels.size(); i++) // parsing output kdl serail chain
{
KDL::Chain chain = kinematicsModels[i];
cout << "Chain " << i << " has " << chain.getNrOfSegments() << " segments" << endl;
for (unsigned int u = 0; u < chain.getNrOfSegments(); u++)
{
KDL::Segment segment = chain.segments[u];
string segmentName = segment.getName();
cout << "Segment " << segmentName << " :" <<endl;
KDL::Frame f_tip = segment.getFrameToTip();
KDL::Vector rotAxis = f_tip.M.GetRot();
double rotAngle = f_tip.M.GetRotAngle(rotAxis);
KDL::Vector trans = f_tip.p;
cout << " frame: rotation " << rotAxis.x() << " " << rotAxis.y() << " " << rotAxis.z() << " " << rotAngle * 180 / M_PI << endl;
cout << " frame: translation " << trans.x() << " " << trans.y() << " " << trans.z() << endl;
KDL::RigidBodyInertia inertia = segment.getInertia();
KDL::Joint joint = segment.getJoint();
string jointName = joint.getName();
string jointType = joint.getTypeName();
KDL::Vector jointAxis = joint.JointAxis();
KDL::Vector jointOrigin = joint.JointOrigin();
cout << " joint name: " << jointName << endl;
cout << " type: " << jointType << endl;
cout << " axis: " << jointAxis.x() << " " <<jointAxis.y() << " " << jointAxis.z() << endl;
cout << " origin: " << jointOrigin.x() << " " << jointOrigin.y() << " " << jointOrigin.z() << endl;
}
}
return 0;
}
void getKDLChainInfo(kinematics_msgs::KinematicSolverInfo &chain_info)
{
unsigned int nj = chain.getNrOfJoints();
unsigned int nl = chain.getNrOfSegments();
ROS_DEBUG("nj: %d", nj);
ROS_DEBUG("nl: %d", nl);
//---setting up response
//joint_names
for(unsigned int i=0; i<nj; i++)
{
ROS_DEBUG("joint_name[%d]: %s", i, chain.getSegment(i).getJoint().getName().c_str());
chain_info.joint_names.push_back(chain.getSegment(i).getJoint().getName());
}
//limits
//joint limits are only saved in KDL::ChainIkSolverPos_NR_JL iksolverpos -> ik_solve().....but this is not visible here!
/*for(unsigned int i=0; i<nj; i++)
{
chain_info.limits[i].joint_name=chain.getSegment(i).getJoint().getName();
chain_info.limits[i].has_position_limits=
chain_info.limits[i].min_position=
chain_info.limits[i].max_position=
chain_info.limits[i].has_velocity_limits=
chain_info.limits[i].max_velocity=
chain_info.limits[i].has_acceleration_limits=
chain_info.limits[i].max_acceleration=
}*/
//link_names
for(unsigned int i=0; i<nl; i++)
{
chain_info.link_names.push_back(chain.getSegment(i).getName());
}
}
void computeRNEDynamicsForChain(KDL::Tree& a_tree, const std::string& rootLink, const std::string& tipLink, KDL::Vector& grav,
std::vector<kdle::JointState>& jointState, std::vector<kdle::SegmentState>& linkState)
{
KDL::Chain achain;
a_tree.getChain(rootLink, tipLink, achain);
KDL::JntArray q(achain.getNrOfJoints());
KDL::JntArray q_dot(achain.getNrOfJoints());
KDL::JntArray q_dotdot(achain.getNrOfJoints());
JntArray torques(achain.getNrOfJoints());
KDL::Wrenches f_ext;
f_ext.resize(achain.getNrOfSegments());
std::cout << endl << endl;
printf("RNE dynamics values \n");
KDL::ChainIdSolver_RNE *rneDynamics = new ChainIdSolver_RNE(achain, -grav);
for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
{
q(i) = jointState[i].q;
q_dot(i) = jointState[i].qdot;
q_dotdot(i) = jointState[i].qdotdot;
printf("q, qdot %f, %f\n", q(i), q_dot(i));
}
rneDynamics->CartToJnt(q, q_dot, q_dotdot, f_ext, torques);
for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
{
printf("index, q, qdot, torques %d, %f, %f, %f\n", i, q(i), q_dot(i), torques(i));
}
return;
}
ChainKinematics::ChainKinematics(const KDL::Chain chain, const double lambda) :
kinChain_(chain),
Njnt_(chain.getNrOfJoints()),
Nsegment_(chain.getNrOfSegments()) {
initialize(lambda);
}
KDL::Wrenches idynChainGet_usingKDL_aux(iCub::iDyn::iDynChain & idynChain, iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
yarp::sig::Vector q,dq,ddq;
q = idynChain.getAng();
dq = idynChain.getDAng();
ddq = idynChain.getD2Ang();
KDL::Chain kdlChain;
std::vector<std::string> la_joints;
la_joints.push_back("left_shoulder_pitch");
la_joints.push_back("left_shoulder_roll");
la_joints.push_back("left_arm_ft_sensor");
la_joints.push_back("left_shoulder_yaw");
la_joints.push_back("left_elbow");
la_joints.push_back("left_wrist_prosup");
la_joints.push_back("left_wrist_pitch");
la_joints.push_back("left_wrist_yaw");
idynSensorChain2kdlChain(idynChain,idynSensor,kdlChain,la_joints,la_joints);
std::cout << kdlChain << std::endl;
int nj = idynChain.getN();
//cout << "idynChainGet_usingKDL_aux with sensor" << " nrJoints " << kdlChain.getNrOfJoints() << " nrsegments " << kdlChain.getNrOfSegments() << endl;
assert(nj==kdlChain.getNrOfJoints());
assert(nj+1==kdlChain.getNrOfSegments());
KDL::JntArray jointpositions = KDL::JntArray(nj);
KDL::JntArray jointvel = KDL::JntArray(nj);
KDL::JntArray jointacc = KDL::JntArray(nj);
KDL::JntArray torques = KDL::JntArray(nj);
for(unsigned int i=0;i<nj;i++){
jointpositions(i)=q[i];
jointvel(i) = dq[i];
jointacc(i) = ddq[i];
}
KDL::Wrenches f_ext(nj+1);
KDL::Wrenches f_int(nj+1);
KDL::Vector grav_kdl;
idynVector2kdlVector(idynChain.getH0().submatrix(0,2,0,2).transposed()*ddp0,grav_kdl);
KDL::ChainIdSolver_RNE_IW neSolver = KDL::ChainIdSolver_RNE_IW(kdlChain,-grav_kdl);
int ret = neSolver.CartToJnt_and_internal_wrenches(jointpositions,jointvel,jointacc,f_ext,torques,f_int);
assert(ret >= 0);
return f_int;
}
void getKDLChainInfo(const KDL::Chain &chain,
kinematics_msgs::KinematicSolverInfo &chain_info)
{
int i=0; // segment number
while(i < (int)chain.getNrOfSegments())
{
chain_info.link_names.push_back(chain.getSegment(i).getName());
i++;
}
}
int Kinematics::getKDLSegmentIndex(const std::string &name) {
int i=0;
while (i < (int)chain.getNrOfSegments()) {
if (chain.getSegment(i).getName() == name) {
return i+1;
}
i++;
}
return -1;
}
int ExcavaROBArmKinematics::getKDLSegmentIndex(const std::string &name) {
int i=0;
while (i < (int) arm_chain_.getNrOfSegments()) {
if (arm_chain_.getSegment(i).getName() == name) {
return i + 1;
}
i++;
}
return -1;
}
sensor_msgs::JointState init_message(const KDL::Chain &chain) {
sensor_msgs::JointState msg;
for (unsigned int i=0; i < chain.getNrOfSegments(); ++i) {
KDL::Segment segment = chain.getSegment(i);
KDL::Joint joint = segment.getJoint();
if (joint.getType() == KDL::Joint::JointType::None) continue;
msg.name.push_back(joint.getName());
msg.position.push_back(0);
}
return msg;
}
FkLookup::ChainFK::ChainFK(const KDL::Tree& tree, const std::string& root, const std::string& tip):root_name(root),tip_name(tip){
KDL::Chain chain;
tree.getChain(root_name, tip_name, chain);
solver = new KDL::ChainFkSolverPos_recursive(chain);
unsigned int num = chain.getNrOfSegments();
for(unsigned int i = 0; i < num; ++i){
const KDL::Joint &joint = chain.getSegment(i).getJoint();
if (joint.getType() != KDL::Joint::None) joints.insert(std::make_pair(joint.getName(),joints.size()));
}
ROS_ASSERT(joints.size() == chain.getNrOfJoints());
}
void printKDLchain(std::string s,const KDL::Chain & kdlChain)
{
std::cout << s << std::endl;
for(int p=0; p < (int)kdlChain.getNrOfSegments(); p++)
{
std::cout << "Segment " << p << ":" << std::endl;
KDL::Segment seg = kdlChain.getSegment(p);
KDL::Frame fra = seg.getFrameToTip();
std::cout << seg << std::endl;
std::cout << fra << std::endl;
}
}
bool leatherman::getSegmentIndex(const KDL::Chain &c, std::string name, int &index)
{
for(size_t j = 0; j < c.getNrOfSegments(); ++j)
{
if(c.getSegment(j).getName().compare(name) == 0)
{
index = j;
return true;
}
}
return false;
}
IkSolver::IkSolver(const KDL::Chain& chain)
{
const std::string root_name = chain.getSegment(0).getName();
const std::string tip_name = chain.getSegment(chain.getNrOfSegments() - 1).getName();
KDL::Tree tree("root");
tree.addChain(chain, "root");
std::vector<std::string> endpoint_names(1, tip_name);
std::vector<EndpointCoupling> endpoint_couplings(1, Pose);
init(tree, endpoint_names, endpoint_couplings);
}
bool ExcavaROBArmKinematics::getPositionFK(excavaROB_arm_kinematics_msgs::GetPositionFK::Request &request,
excavaROB_arm_kinematics_msgs::GetPositionFK::Response &response) {
KDL::Frame p_out;
KDL::JntArray jnt_pos_in;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
if (num_joints_arm_ != request.fk_link_names.size()) {
ROS_ERROR("The request has not the same dimension of the arm_chain joints.");
return false;
}
jnt_pos_in.resize(num_joints_arm_);
for (unsigned int i = 0; i < num_joints_arm_; i++) {
int jnt_index = getJointIndex(request.joint_state.name[i]);
if (jnt_index >= 0)
jnt_pos_in(jnt_index) = request.joint_state.position[i];
}
response.pose_stamped.resize(num_joints_arm_);
response.fk_link_names.resize(num_joints_arm_);
bool valid = true;
for (unsigned int i = 0; i < num_joints_arm_; i++) {
int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
ROS_DEBUG("End effector index: %d", segmentIndex);
ROS_DEBUG("Chain indices: %d", arm_chain_.getNrOfSegments());
if (fk_solver_->JntToCart(jnt_pos_in, p_out, segmentIndex) >= 0) {
tf_pose.frame_id_ = root_name_;
tf_pose.stamp_ = ros::Time();
tf::PoseKDLToTF(p_out, tf_pose);
try {
tf_listener_.transformPose(request.header.frame_id, tf_pose, tf_pose);
} catch (...) {
ROS_ERROR("ExcavaROB Kinematics: Could not transform FK pose to frame: %s", request.header.frame_id.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::poseStampedTFToMsg(tf_pose, pose);
response.pose_stamped[i] = pose;
response.fk_link_names[i] = request.fk_link_names[i];
response.error_code.val = response.error_code.SUCCESS;
} else {
ROS_ERROR("ExcavaROB Kinematics: Could not compute FK for %s", request.fk_link_names[i].c_str());
response.error_code.val = response.error_code.NO_FK_SOLUTION;
valid = false;
}
}
return true;
}
int getKDLSegmentIndex(const KDL::Chain &chain,
const std::string &name)
{
int i=0; // segment number
while(i < (int)chain.getNrOfSegments())
{
if(chain.getSegment(i).getName() == name)
{
return i+1;
}
i++;
}
return -1;
}
bool addBaseTransformation(const KDL::Chain & old_chain, KDL::Chain & new_chain, KDL::Frame H_new_old)
{
new_chain = KDL::Chain();
for(unsigned int i=0; i<old_chain.getNrOfSegments(); i++) {
KDL::Segment segm;
segm = old_chain.getSegment(i);
//if is not the first segment add normally the segment
if( i != 0 ) {
new_chain.addSegment(segm);
} else {
//otherwise modify the segment before adding it
KDL::Segment new_segm;
KDL::Joint new_joint, old_joint;
old_joint = segm.getJoint();
KDL::Joint::JointType new_type;
switch(old_joint.getType()) {
case KDL::Joint::RotAxis:
case KDL::Joint::RotX:
case KDL::Joint::RotY:
case KDL::Joint::RotZ:
new_type = KDL::Joint::RotAxis;
break;
case KDL::Joint::TransAxis:
case KDL::Joint::TransX:
case KDL::Joint::TransY:
case KDL::Joint::TransZ:
new_type = KDL::Joint::TransAxis;
break;
case KDL::Joint::None:
default:
new_type = KDL::Joint::None;
}
//check !
new_joint = KDL::Joint(old_joint.getName(),H_new_old*old_joint.JointOrigin(),H_new_old.M*old_joint.JointAxis(),new_type);
new_segm = KDL::Segment(segm.getName(),new_joint,H_new_old*segm.getFrameToTip(),segm.getInertia());
new_chain.addSegment(new_segm);
}
}
return true;
}
bool OrientationConstraint::initialize(const arm_navigation_msgs::OrientationConstraint& constraint,
const KDL::Chain& chain)
{
constraint_ = constraint;
link_id_ = -1;
for (unsigned int i=0; i<chain.getNrOfSegments(); ++i)
{
if (constraint_.link_name == chain.getSegment(i).getName())
{
link_id_ = i;
break;
}
}
if (link_id_ == -1)
{
ROS_ERROR("OrientationConstraint: couldn't find link %s in chain.", constraint_.link_name.c_str());
return false;
}
return true;
}
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;
}
}
}
bool PositionConstraint::initialize(const arm_navigation_msgs::PositionConstraint& constraint,
const KDL::Chain& chain)
{
constraint_ = constraint;
link_id_ = -1;
for (unsigned int i=0; i<chain.getNrOfSegments(); ++i)
{
if (constraint_.link_name == chain.getSegment(i).getName())
{
link_id_ = i;
break;
}
}
if (link_id_ == -1)
{
ROS_ERROR("CIK PositionConstraint: couldn't find link %s in chain.", constraint_.link_name.c_str());
return false;
}
rosPointToKdlVector(constraint_.target_point_offset, offset_);
rosPointToKdlVector(constraint_.position, desired_location_);
if (constraint_.constraint_region_shape.type != arm_navigation_msgs::Shape::BOX ||
constraint_.constraint_region_shape.dimensions.size() != 3)
{
ROS_ERROR("CIK PositionConstraint: we only handle 3-d BOX constraints currently.");
return false;
}
KDL::Rotation constraint_rotation;
rosQuaternionToKdlRotation(constraint_.constraint_region_orientation, constraint_rotation);
constraint_rotation_inverse_ = constraint_rotation.Inverse();
kdlRotationToEigenMatrix3d(constraint_rotation_inverse_, constraint_rotation_inverse_eigen_);
return true;
}
void callbackJointState(const JointStateConstPtr& state)
{
std::map<std::string, double> joint_name_position;
if (state->name.size() != state->position.size()) {
ROS_ERROR("Robot state publisher received an invalid joint state vector");
return;
}
// determine least recently published joint
ros::Time last_published = ros::Time::now();
for (unsigned int i=0; i<state->name.size(); i++) {
ros::Time t = last_publish_time_[state->name[i]];
last_published = (t < last_published) ? t : last_published;
}
if (state->header.stamp >= last_published + publish_interval_) {
// get joint positions from state message
std::map<std::string, double> joint_positions;
std::map<std::string, double> joint_efforts;
for (unsigned int i=0; i<state->name.size(); i++) {
joint_positions.insert(make_pair(state->name[i], state->position[i]));
joint_efforts.insert(make_pair(state->name[i], state->effort[i]));
}
KDL::JntArray tau;
KDL::Wrench F;
Eigen::Matrix<double,Eigen::Dynamic,6> jac_t;
Eigen::Matrix<double,6,Eigen::Dynamic> jac_t_pseudo_inv;
tf::StampedTransform transform;
KDL::Wrench F_pub;
tf::Vector3 tf_force;
tf::Vector3 tf_torque;
tau.resize(chain_.getNrOfJoints());
//getPositions;
for (size_t i=0, j=0; i<chain_.getNrOfSegments(); i++) {
if (chain_.getSegment(i).getJoint().getType() == KDL::Joint::None)
continue;
// check
std::string joint_name = chain_.getSegment(i).getJoint().getName();
if ( joint_positions.find(joint_name) == joint_positions.end() ) {
ROS_ERROR("Joint '%s' is not found in joint state vector", joint_name.c_str());
}
// set position
jnt_pos_(j) = joint_positions[joint_name];
tau(j) = joint_efforts[joint_name];
j++;
}
jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
jac_t = jacobian_.data.transpose();
if ( jacobian_.columns() >= jacobian_.rows() ) {
jac_t_pseudo_inv =(jac_t.transpose() * jac_t).inverse() * jac_t.transpose();
} else {
jac_t_pseudo_inv =jac_t.transpose() * ( jac_t * jac_t.transpose() ).inverse();
}
#if 1
{
ROS_INFO("jac_t# jac_t : ");
Eigen::Matrix<double,6,6> mat_i = mat_i = jac_t_pseudo_inv * jac_t;
for (unsigned int i = 0; i < 6; i++) {
std::stringstream ss;
for (unsigned int j=0; j<6; j++)
ss << std::fixed << std::setw(8) << std::setprecision(4) << mat_i(j,i) << " ";
ROS_INFO_STREAM(ss.str());
}
}
#endif
// f = - inv(jt) * effort
for (unsigned int j=0; j<6; j++)
{
F(j) = 0;
for (unsigned int i = 0; i < jnt_pos_.rows(); i++)
{
F(j) += -1 * jac_t_pseudo_inv(j, i) * tau(i);
}
}
//low pass filter
F += (F_pre_ - F) * exp(-1.0 / t_const_);
for (unsigned int j=0; j<6; j++) {
F_pre_(j) = 0;
}
F_pre_ += F;
//tf transformation
tf::vectorKDLToTF(F.force, tf_force);
tf::vectorKDLToTF(F.torque, tf_torque);
try {
listener_.waitForTransform( tip, root, state->header.stamp, ros::Duration(1.0));
listener_.lookupTransform( tip, root, state->header.stamp , transform);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
for (unsigned int j=0; j<3; j++) {
F_pub.force[j] = transform.getBasis()[j].dot(tf_force);
F_pub.torque[j] = transform.getBasis()[j].dot(tf_torque);
}
geometry_msgs::WrenchStamped msg;
msg.header.stamp = state->header.stamp;
msg.header.frame_id = tip;
// http://code.ros.org/lurker/message/20110107.151127.7177af06.nl.html
//tf::WrenchKDLToMsg(F,msg.wrench);
msg.wrench.force.x = F_pub.force[0];
msg.wrench.force.y = F_pub.force[1];
msg.wrench.force.z = F_pub.force[2];
msg.wrench.torque.x = F_pub.torque[0];
msg.wrench.torque.y = F_pub.torque[1];
msg.wrench.torque.z = F_pub.torque[2];
wrench_stamped_pub_.publish(msg);
{
ROS_INFO("jacobian : ");
for (unsigned int i = 0; i < jnt_pos_.rows(); i++) {
std::stringstream ss;
for (unsigned int j=0; j<6; j++)
ss << std::fixed << std::setw(8) << std::setprecision(4) << jacobian_(j,i) << " ";
ROS_INFO_STREAM(ss.str());
}
ROS_INFO("effort : ");
std::stringstream sstau;
for (unsigned int i = 0; i < tau.rows(); i++) {
sstau << std::fixed << std::setw(8) << std::setprecision(4) << tau(i) << " ";
}
ROS_INFO_STREAM(sstau.str());
ROS_INFO("force : ");
std::stringstream ssf;
for (unsigned int j = 0; j < 6; j++) {
ssf << std::fixed << std::setw(8) << std::setprecision(4) << F(j) << " ";
}
ROS_INFO_STREAM(ssf.str());
}
// store publish time in joint map
for (unsigned int i=0; i<state->name.size(); i++)
last_publish_time_[state->name[i]] = state->header.stamp;
}
}
int main()
{
Vector Fend(3); Fend.zero();
Vector Muend(Fend);
Vector w0(3); Vector dw0(3); Vector ddp0(3);
w0 = dw0=ddp0=0.0; ddp0[1]=g;
int N = 7;
Vector q(N);
Vector dq(N);
Vector ddq(N);
Random rng;
rng.seed(yarp::os::Time::now());
for(int i=0;i<N-1;i++)
{
q[i] = 0*CTRL_DEG2RAD*rng.uniform();
dq[i] = 0*CTRL_DEG2RAD*rng.uniform();
ddq[i] = 0*CTRL_DEG2RAD*rng.uniform();
}
for(int i=N-1;i<N;i++)
{
q[i] = 0;
dq[i] = 0;
ddq[i] = 1;
}
L5PMDyn threeChain;
iDynInvSensorL5PM threeChainSensor(threeChain.asChain(),"papare",DYNAMIC,iCub::skinDynLib::VERBOSE);
int sensor_link = threeChainSensor.getSensorLink();
q = threeChain.setAng(q);
dq = threeChain.setDAng(dq);
ddq = threeChain.setD2Ang(ddq);
int nj=N;
KDL::JntArray jointpositions = KDL::JntArray(nj);
KDL::JntArray jointvel = KDL::JntArray(nj);
KDL::JntArray jointacc = KDL::JntArray(nj);
KDL::JntArray torques = KDL::JntArray(nj);
for(unsigned int i=0;i<nj;i++){
jointpositions(i)=q[i];
jointvel(i) = dq[i];
jointacc(i) = ddq[i];
}
threeChain.prepareNewtonEuler(DYNAMIC);
threeChain.computeNewtonEuler(w0,dw0,ddp0,Fend,Muend);
threeChainSensor.computeSensorForceMoment();
// then we can retrieve our results...
// forces moments and torques
Matrix F = threeChain.getForces();
Matrix Mu = threeChain.getMoments();
Vector Tau = threeChain.getTorques();
Matrix F_KDL = idynChainGetForces_usingKDL(threeChain,ddp0);
Matrix Mu_KDL = idynChainGetMoments_usingKDL(threeChain,ddp0);
Vector Tau_KDL = idynChainGetTorques_usingKDL(threeChain,ddp0);
Matrix F_KDL_sens = idynChainGetForces_usingKDL(threeChain,threeChainSensor,ddp0);
Matrix Mu_KDL_sens = idynChainGetMoments_usingKDL(threeChain,threeChainSensor,ddp0);
Vector Tau_KDL_sens = idynChainGetTorques_usingKDL(threeChain,threeChainSensor,ddp0);
Matrix p_idyn(3,N),p_kdl_no_sens(3,N),p_kdl_sens(3,N+1);
for(int l=0;l<N;l++)
{
p_idyn.setCol(l,threeChain.getH(l).subcol(0,3,3));
}
KDL::Chain threeChainKDL;
std::vector<std::string> dummy;
std::string dummy_str = "";
for(int l=0;l<N;l++) {
Vector p_kdl;
idynChain2kdlChain(*(threeChain.asChain()),threeChainKDL,dummy,dummy,dummy_str,dummy_str,l+1);
KDL::ChainFkSolverPos_recursive posSolver = KDL::ChainFkSolverPos_recursive(threeChainKDL);
KDL::Frame cartpos;
KDL::JntArray jpos;
jpos.resize(l+1);
for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
posSolver.JntToCart(jpos,cartpos);
to_iDyn(cartpos.p,p_kdl);
p_kdl_no_sens.setCol(l,p_kdl);
}
KDL::Chain threeChainKDLsens;
for(int l=0;l<N+1;l++) {
Vector p_kdl;
idynSensorChain2kdlChain(*(threeChain.asChain()),threeChainSensor,threeChainKDLsens,dummy,dummy,dummy_str,dummy_str,l+1);
KDL::ChainFkSolverPos_recursive posSolver = KDL::ChainFkSolverPos_recursive(threeChainKDLsens);
KDL::Frame cartpos;
KDL::JntArray jpos;
if( l <= sensor_link ) {
jpos.resize(l+1);
for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
}
//if( l >= sensor_link )
else {
jpos.resize(l);
for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
}
cout << "l " << l << " nrJoints " << threeChainKDLsens.getNrOfJoints() << " nrsegments " << threeChainKDLsens.getNrOfSegments() <<" jpos dim " << jpos.rows() << endl;
assert(posSolver.JntToCart(jpos,cartpos) >= 0);
to_iDyn(cartpos.p,p_kdl);
p_kdl_sens.setCol(l,p_kdl);
}
printMatrix("p_idyn",p_idyn);
printMatrix("p_kdl_no_sens",p_kdl_no_sens);
printMatrix("p_kdl_sens",p_kdl_sens);
cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~`KDL Chain~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``" << endl << threeChainKDL << endl;
cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~KDL Chain sens~~~~~~~~~~~~~~~~~~~~~~~~~~~~`" << endl << threeChainKDLsens << endl;
//printKDLchain("threeChainKDLsens",threeChainKDLsens);
printMatrix("F",F);
printMatrix("F_KDL",F_KDL);
printMatrix("F_KDL_sens",F_KDL_sens);
printMatrix("Mu",Mu);
printMatrix("Mu_KDL",Mu_KDL);
printMatrix("Mu_KDL_sens",Mu_KDL_sens);
printVector("Tau",Tau);
printVector("Tau_KDL",Tau_KDL);
printVector("Tau_KDL_sens",Tau_KDL_sens);
for(int l=0;l<F_KDL.cols();l++)
{
YARP_ASSERT(EQUALISH(F_KDL.getCol(l),F.getCol(l)));
YARP_ASSERT(EQUALISH(F_KDL_sens.getCol(l),F.getCol(l)));
YARP_ASSERT(EQUALISH(Mu_KDL.getCol(l),Mu.getCol(l)));
YARP_ASSERT(EQUALISH(Mu_KDL_sens.getCol(l),Mu.getCol(l)));
}
for(int l=0;l<Tau.size();l++) {
YARP_ASSERT(abs(Tau(l)-Tau_KDL(l))<delta);
YARP_ASSERT(abs(Tau(l)-Tau_KDL_sens(l))<delta);
}
}
bool idynSensorChain2kdlChain(iCub::iDyn::iDynChain & idynChain,
iCub::iDyn::iDynInvSensor & idynSensor,
KDL::Chain & kdlChain,
KDL::Frame & H_sensor_dh_child,
std::vector<std::string> link_names,std::vector<std::string> joint_names, std::string final_frame_name, std::string initial_frame_name, int max_links)
{
bool use_names;
int n_links, i, sensor_link;
n_links = idynChain.getN();
sensor_link = idynSensor.getSensorLink();
int kdl_links = n_links + 1; //The sensor links transform a link in two different links
int kdl_joints = kdl_links;
if(n_links <= 0 ) return false;
if( (int)link_names.size() < kdl_links || (int)joint_names.size() < kdl_joints ) {
use_names = false;
} else {
use_names = true;
}
KDL::Frame kdlFrame = KDL::Frame();
KDL::Frame kdl_H;
kdlChain = KDL::Chain();
KDL::Segment kdlSegment = KDL::Segment();
KDL::RigidBodyInertia kdlRigidBodyInertia = KDL::RigidBodyInertia();
int kdl_i = 0;
if( initial_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getH0(),kdlFrame);
kdlSegment = KDL::Segment(initial_frame_name,KDL::Joint(initial_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
KDL::Frame remainder_frame = KDL::Frame::Identity();
for(i=0; i<n_links; i++)
{
if( i != sensor_link ) {
//forgive him, as he does not know what is doing
iCub::iKin::iKinLink & link_current = idynChain[i];
//For the last link, take in account also HN
if ( i == n_links - 1) {
idynMatrix2kdlFrame(idynChain.getHN(),kdl_H);
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset())*kdl_H;
} else {
kdlFrame = kdlFrame.DH(link_current.getA(),link_current.getAlpha(),link_current.getD(),link_current.getOffset());
}
bool ret = idynDynamicalParameters2kdlRigidBodyInertia(idynChain.getMass(i),idynChain.getCOM(i).subcol(0,3,3),idynChain.getInertia(i),kdlRigidBodyInertia);
assert(ret);
if(!ret) return false;
//For the joint after the ft sensor, consider the offset between the ft sensor
// and the joint
if( idynChain.isLinkBlocked(i) ) {
// if it is blocked, it is easy to add the remainder frame
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),remainder_frame*kdlFrame,kdlRigidBodyInertia);
}
} else {
KDL::Vector new_joint_axis, new_joint_origin;
new_joint_axis = remainder_frame.M*KDL::Vector(0.0,0.0,0.1);
new_joint_origin = remainder_frame.p;
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(new_joint_origin,new_joint_axis,KDL::Joint::RotAxis),remainder_frame*kdlFrame,kdlRigidBodyInertia);
}
}
kdlChain.addSegment(kdlSegment);
remainder_frame = KDL::Frame::Identity();
} else {
//( i == segment_link )
double m,m0,m1;
yarp::sig::Vector r0(3),r1(3),r(3),rgg0(3),rgg1(3),r_i_s_wrt_i(3),r_i_C0_wrt_i;
yarp::sig::Matrix I,I0,I1;
yarp::sig::Matrix R_s_wrt_i;
iCub::iKin::iKinLink & link_current = idynChain[sensor_link];
KDL::Frame kdlFrame_0 = KDL::Frame();
KDL::Frame kdlFrame_1 = KDL::Frame();
//Imagine that we have i, s , i+1
//yarp::sig::Matrix H_0;
//The angle of the sensor link joint is put to 0 and then restored
double angSensorLink = link_current.getAng();
yarp::sig::Matrix H_sensor_link = (link_current.getH(0.0)); //H_{i-1}_i
link_current.setAng(angSensorLink);
//idynSensor.getH() <--> H_i_s
yarp::sig::Matrix H_0 = H_sensor_link * (idynSensor.getH()); // H_{i-1}_s = H_{i-1}_i*H_i_s ?
yarp::sig::Matrix H_1 = localSE3inv(idynSensor.getH()); //H_s_{i}
//std::cout << "H_0" << std::endl << H_0.toString() << std::endl;
//std::cout << "H_1" << std::endl << H_1.toString() << std::endl;
idynMatrix2kdlFrame(H_0,kdlFrame_0);
idynMatrix2kdlFrame(H_1,kdlFrame_1);
remainder_frame = kdlFrame_1;
H_sensor_dh_child = remainder_frame;
kdlFrame_1 = KDL::Frame::Identity();
//cout << "kdlFrame_0: " << endl;
//cout << kdlFrame_0;
//cout << "kdlFrame_1: " << endl;
//cout << kdlFrame_1;
KDL::RigidBodyInertia kdlRigidBodyInertia_0 = KDL::RigidBodyInertia();
KDL::RigidBodyInertia kdlRigidBodyInertia_1 = KDL::RigidBodyInertia();
m = idynChain.getMass(sensor_link);
m1 = idynSensor.getMass();
m0 = m-m1;
//It is not possible that the semilink is more heavy than the link!!!
assert(m0 > 0);
//r_{i,C_i}^i
r = idynChain.getCOM(i).subcol(0,3,3);
//R_s^i
R_s_wrt_i = idynSensor.getH().submatrix(0,2,0,2);
r_i_s_wrt_i = idynSensor.getH().subcol(0,3,3);
//r0 := r_{s,C_{{vl}_0}}^s
//r1 := r_{i,C_{{vl}_1}}^i
//
r1 = r_i_s_wrt_i + R_s_wrt_i*(idynSensor.getCOM().subcol(0,3,3));
//cout << "m0: " << m0 << endl;
r_i_C0_wrt_i = (1/m0)*(m*r-m1*r1);
r0 = R_s_wrt_i.transposed()*(r_i_C0_wrt_i-r_i_s_wrt_i);
I = idynChain.getInertia(i);
I1 = R_s_wrt_i*idynSensor.getInertia()*R_s_wrt_i.transposed();
rgg0 = -1*r+r_i_C0_wrt_i;
rgg1 = -1*r+r1;
I0 = R_s_wrt_i.transposed()*(I - I1 + m1*crossProductMatrix(rgg1)*crossProductMatrix(rgg1) + m0*crossProductMatrix(rgg0)*crossProductMatrix(rgg0))*R_s_wrt_i;
//DEBUG
//printMatrix("I",I);
//printMatrix("I1",I1);
//printMatrix("I0",I0);
//printMatrix("cross",crossProductMatrix(rgg1));
//cout << "m0: " << m0 << endl;
//printVector("r0",r0);
//printMatrix("I0",I0);
bool ret;
ret = idynDynamicalParameters2kdlRigidBodyInertia(m0,r0,I0,kdlRigidBodyInertia_0);
assert(ret);
if(!ret) return false;
ret = idynDynamicalParameters2kdlRigidBodyInertia(m1,r1,I1,kdlRigidBodyInertia_1);
assert(ret);
if(!ret) return false;
KDL::RigidBodyInertia kdlRigidBodyInertia_1_buf;
kdlRigidBodyInertia_1_buf = remainder_frame*kdlRigidBodyInertia_1;
kdlRigidBodyInertia_1 = kdlRigidBodyInertia_1_buf;
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::RotZ),kdlFrame_0,kdlRigidBodyInertia_0);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::RotZ),kdlFrame_0,kdlRigidBodyInertia_0);
}
kdlChain.addSegment(kdlSegment);
if( use_names ) {
kdlSegment = KDL::Segment(link_names[kdl_i],KDL::Joint(joint_names[kdl_i],KDL::Joint::None),kdlFrame_1,kdlRigidBodyInertia_1);
kdl_i++;
} else {
kdlSegment = KDL::Segment(KDL::Joint(KDL::Joint::None),kdlFrame_1,kdlRigidBodyInertia_1);
}
kdlChain.addSegment(kdlSegment);
}
}
//Final link can be segment
if( final_frame_name.length() != 0 ) {
idynMatrix2kdlFrame(idynChain.getHN(),kdlFrame);
kdlSegment = KDL::Segment(final_frame_name,KDL::Joint(final_frame_name+"_joint",KDL::Joint::None),kdlFrame);
kdlChain.addSegment(kdlSegment);
}
if( max_links < (int)kdlChain.getNrOfSegments() )
{
KDL::Chain new_kdlChain;
for(int p=0; p < max_links; p++)
{
new_kdlChain.addSegment(kdlChain.getSegment(p));
}
kdlChain = new_kdlChain;
}
//Considering the H0 transformation
if( initial_frame_name.length() == 0 ) {
KDL::Chain new_chain;
KDL::Frame kdl_H0;
idynMatrix2kdlFrame(idynChain.getH0(),kdl_H0);
addBaseTransformation(kdlChain,new_chain,kdl_H0);
kdlChain = new_chain;
}
return true;
}
bool fk_solve_all(kinematics_msgs::GetPositionFK::Request &req,
kinematics_msgs::GetPositionFK::Response &res )
{
ROS_INFO("[TESTING]: get_fk_all_service has been called!");
unsigned int nj = chain.getNrOfJoints();
unsigned int nl = chain.getNrOfSegments();
ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
JntArray conf(nj);
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
for(int i = 0; i < nj; i++)
conf(i) = req.robot_state.joint_state.position[i];
Frame F_ist;
res.pose_stamped.resize(nl);
res.fk_link_names.resize(nl);
for(int j = 0; j < nl; j++)
{
if(fksolver1.JntToCart(conf, F_ist, j+1) >= 0)
{
std::cout << "Calculated Position out of Configuration for segment " << j+1 << ":\n";
std::cout << F_ist <<"\n";
std::cout << "Calculated Position out of Configuration:\n";
std::cout << F_ist <<"\n";
//TODO: fill out response!!!
tf_pose.frame_id_ = "base_link";//root_name_;
tf_pose.stamp_ = ros::Time();
tf::PoseKDLToTF(F_ist,tf_pose);
try
{
//tf_.transformPose(req.header.frame_id,tf_pose,tf_pose);
}
catch(...)
{
ROS_ERROR("Could not transform FK pose to frame: %s",req.header.frame_id.c_str());
res.error_code.val = res.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::poseStampedTFToMsg(tf_pose,pose);
res.pose_stamped[j] = pose;
res.fk_link_names[j] = chain.getSegment(j).getName();
res.error_code.val = res.error_code.SUCCESS;
}
else
{
ROS_ERROR("Could not compute FK for segment %d", j);
res.error_code.val = res.error_code.NO_FK_SOLUTION;
}
//TODO: fill out response!!!
//res.pose_stamped[j]
}
return true;
}
int main()
{
using namespace KDL;
//Definition of kinematic chain
KDL::Tree youBotTree;
//note that it seems youbot_arm.urdf and youbot.urdf use different values.
//parser's assumption is that it return joints which are not of type None.
if (!kdl_parser::treeFromFile("/home/azamat/programming/ros-diamondback/youbot-ros-pkg/youbot_common/youbot_description/robots/youbot.urdf", youBotTree))
{
std::cout << "Failed to construct kdl tree" << std::endl;
return false;
}
// else
// {
// std::cout << "succeeded " << youBotTree.getNrOfJoints()<< std::endl;
// std::cout << "succeeded " << youBotTree.getNrOfSegments()<< std::endl;
// }
KDL::Chain chain;
youBotTree.getChain("arm_link_0","arm_link_5",chain);
unsigned int numberOfJoints = chain.getNrOfJoints();
unsigned int numberOfLinks = chain.getNrOfSegments();
// std::cout << "number of chain links " << numberOfLinks << std::endl;
// std::cout << "number of chain joints " << numberOfJoints << std::endl;
//~Definition of kinematic chain
//Definition of constraints and external disturbances
//--------------------------------------------------------------------------------------//
//Constraint force matrix at the end-effector
//What is the convention for the spatial force matrix; is it the same as in thesis?
//NOTE AZAMAT: Constraint are also defined in local reference frame?!
unsigned int numberOfConstraints = 2;
Twist constraintForce;
Twists constraintForces;
for (unsigned int i = 0; i < numberOfJoints; i++)
{
SetToZero(constraintForce);
constraintForces.push_back(constraintForce);
}
constraintForces[0].vel[0] = 0;
constraintForces[1].vel[1] = 0;
constraintForces[2].vel[2] = 0;
#ifdef X_CONSTRAINT_SET
constraintForces[0].vel[0] = 1;
#endif //~X_CONSTRAINT_SET
#ifdef Y_CONSTRAINT_SET
constraintForces[1].vel[1] = 1;
#endif //~Y_CONSTRAINT_SET
#ifdef Z_CONSTRAINT_SET
constraintForces[2].vel[2] = 1;
#endif //~Z_CONSTRAINT_SET
//Acceleration energy and constraint matrix at the end-effector
Jacobian alpha(numberOfConstraints);
JntArray betha(numberOfConstraints); //set to zero
JntArray bethaControl(numberOfConstraints);
for (unsigned int i = 0; i < numberOfConstraints; i++)
{
alpha.setColumn(i, constraintForces[i]);
betha(i) = 0.0;
bethaControl(i) = 0.0;
}
//arm root acceleration
Vector linearAcc(0.0, 0.0, 9.8); //gravitational acceleration along Z
Vector angularAcc(0.0, 0.0, 0.0);
Twist twist0(linearAcc, angularAcc);
//external forces on the arm
Wrench externalNetForce;
Wrenches externalNetForces;
for (unsigned int i = 0; i < numberOfLinks; i++)
{
externalNetForces.push_back(externalNetForce);
}
//~Definition of constraints and external disturbances
//Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
ChainIdSolver_Vereshchagin constraintSolver(chain, twist0, numberOfConstraints);
//These arrays of joint values contain actual and desired values
//actual is generated by a solver and integrator
//desired is given by an interpolator
//error is the difference between desired-actual
//0-actual, 1-desired, 2-error, 3-sum of error
unsigned int k = 4;
JntArray jointPoses[k];
JntArray jointRates[k];
JntArray jointAccelerations[k];
JntArray jointTorques[k];
JntArray jointControlTorques[k];
for (unsigned int i = 0; i < k; i++)
{
JntArray jointValues(numberOfJoints);
jointPoses[i] = jointValues;
jointRates[i] = jointValues;
jointAccelerations[i] = jointValues;
jointTorques[i] = jointValues;
jointControlTorques[i] = jointValues;
}
//cartesian space/link values
//0-actual, 1-desire, 2-error, 3-errorsum
Frames cartX[k];
Twists cartXDot[k];
Twists cartXDotDot[k];
Twist accLink;
KDL::Frame frameTemp;
for (unsigned int i = 0; i < k; i++) //i is number of variables (actual, desired, error)
{
for (unsigned int j = 0; j < numberOfLinks; j++) //j is number of links
{
cartX[i].push_back(frameTemp);
cartXDot[i].push_back(accLink);
cartXDotDot[i].push_back(accLink);
}
}
// Initial arm position configuration/constraint, negative in clockwise
JntArray jointInitialPose(numberOfJoints);
jointInitialPose(0) = -M_PI/4.0;
jointInitialPose(1) = M_PI / 6.0;
jointInitialPose(2) = M_PI / 12.0;
jointInitialPose(3) = -M_PI / 3.0;
jointInitialPose(4) = M_PI / 18.0;
//corresponds to x=0.031827 y = -0.007827 z = 0.472972
JntArray jointFinalPose(numberOfJoints);
jointFinalPose(0) = M_PI / 2.0;
jointFinalPose(1) = M_PI / 2.5;
jointFinalPose(2) = 0.0;
jointFinalPose(3) = 0.0;
jointFinalPose(4) = 0.0;
// corresponds to 0.024000 -0.367062 0.242885
for (unsigned int i = 0; i < numberOfJoints; i++)
{
//actual initial
jointPoses[0](i) = jointInitialPose(i);
//desired initial
jointPoses[1](i) = jointInitialPose(i);
}
//test
#ifdef FKPOSE_TEST
ChainFkSolverPos_recursive fksolver(chain);
Frame tempEE;
fksolver.JntToCart(jointPoses[0], tempEE, 5);
std::cout << tempEE << std::endl;
fksolver.JntToCart(jointFinalPose, tempEE, 5);
std::cout << tempEE << std::endl;
#endif //~FKPOSE_TEST
//~Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
//Definition of process main loop
//-------------------------------------------------------------------------------------//
double taskTimeConstant = 2; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
double simulationTime = 2.5 * taskTimeConstant;
double timeDelta = 0.001;
double timeToSettle = 0.5;
int status;
double alfa(0.0), beta(0.0), gamma(0.0);
//Controller gain configurations
double ksi[3] = {1.0, 1.0, 1.0}; //damping factor
double Kp[3];
Kp[0] = 240.1475/(timeToSettle*timeToSettle);//0.1475
Kp[1] = 140.0/(timeToSettle*timeToSettle);
Kp[2] = 240.0/(timeToSettle*timeToSettle);
double Kv[3];
Kv[0] = 55.2245*ksi[0]/timeToSettle;//2.2245
Kv[1] = 30.6*ksi[1]/timeToSettle;
Kv[2] = 30.6*ksi[1]/timeToSettle;
double Ki[3] = {0.0, 0.0, 0.0};
double K = 0.005;
//For joint space control without constraints using computed torque
// double ksi[2] = {1.0, 1.0}; //damping factor
double Kpq[numberOfJoints];
Kpq[0] = 2.5/(timeToSettle*timeToSettle);
Kpq[1] = 2.2/(timeToSettle*timeToSettle);
Kpq[2] = 20.1/(timeToSettle*timeToSettle);
Kpq[3] = 40.1/(timeToSettle*timeToSettle);
Kpq[4] = 40.1/(timeToSettle*timeToSettle);
double Kvq[numberOfJoints];
Kvq[0] = .5*ksi[0]/timeToSettle;
Kvq[1] = 1.0*ksi[1]/timeToSettle;
Kvq[2] = 3.0*ksi[1]/timeToSettle;
Kvq[3] = 3.0*ksi[1]/timeToSettle;
Kvq[4] = 3.0*ksi[1]/timeToSettle;
//Interpolator parameters:
// Initial x=0.031827 y = -0.007827 z = 0.472972
// Final 0.024000 -0.367062 0.242885
//cartesian space
double b0_x = 0.031827; //should come from initial joint configuration
double b1_x = 0.0;
double b2_x = ((0.031827 - b0_x)*3.0 / (simulationTime * simulationTime));
double b3_x = -((0.031827 - b0_x)*2.0 / (simulationTime * simulationTime * simulationTime));
double b0_y = -0.007827; //should come from initial joint configuration
double b1_y = 0.0;
double b2_y = ((-0.007827 - b0_y)*3.0 / (simulationTime * simulationTime));
double b3_y = -((-0.007827 - b0_y)*2.0 / (simulationTime * simulationTime * simulationTime));
double b0_z = 0.472972; //should come from initial joint configuration
double b1_z = 0.0;
double b2_z = ((0.242885 - b0_z)*3.0 / (simulationTime * simulationTime));
double b3_z = -((0.242885 - b0_z)*2.0 / (simulationTime * simulationTime * simulationTime));
//joint space
double a0_q0 = jointInitialPose(0);
double a1_q0 = 0.0;
double a2_q0 = ((jointFinalPose(0) - jointInitialPose(0))*3.0 / (simulationTime * simulationTime));
double a3_q0 = -((jointFinalPose(0) - jointInitialPose(0))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q1 = jointInitialPose(1);
double a1_q1 = 0.0;
double a2_q1 = ((jointFinalPose(1) - jointInitialPose(1))*3.0 / (simulationTime * simulationTime));
double a3_q1 = -((jointFinalPose(1) - jointInitialPose(1))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q2 = jointInitialPose(2);
double a1_q2 = 0.0;
double a2_q2 = ((jointFinalPose(2) - jointInitialPose(2))*3.0 / (simulationTime * simulationTime));
double a3_q2 = -((jointFinalPose(2) - jointInitialPose(2))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q3 = jointInitialPose(3);
double a1_q3 = 0.0;
double a2_q3 = ((jointFinalPose(3) - jointInitialPose(3))*3.0 / (simulationTime * simulationTime));
double a3_q3 = -((jointFinalPose(3) - jointInitialPose(3))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q4 = jointInitialPose(4);
double a1_q4 = 0.0;
double a2_q4 = ((jointFinalPose(4) - jointInitialPose(4))*3.0 / (simulationTime * simulationTime));
double a3_q4 = -((jointFinalPose(4) - jointInitialPose(4))*2.0 / (simulationTime * simulationTime * simulationTime));
double feedforwardJointTorque0 = 0.0;
double feedforwardJointTorque1 = 0.0;
double feedforwardJointTorque2 = 0.0;
double feedforwardJointTorque3 = 0.0;
double feedforwardJointTorque4 = 0.0;
//---------------------------------------------------------------------------------------//
//Main simulation loop
for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
{
//Interpolation (Desired) q = a0+a1t+a2t^2+a3t^3
cartX[1][4].p[0] = b0_x + b1_x * t + b2_x * t * t + b3_x * t * t*t;
cartX[1][4].p[1] = b0_y + b1_y * t + b2_y * t * t + b3_y * t * t*t;
cartX[1][4].p[2] = b0_z + b1_z * t + b2_z * t * t + b3_z * t * t*t;
cartXDot[1][4].vel[0] = b1_x + 2 * b2_x * t + 3 * b3_x * t*t;
cartXDot[1][4].vel[1] = b1_y + 2 * b2_y * t + 3 * b3_y * t*t;
cartXDot[1][4].vel[2] = b1_z + 2 * b2_z * t + 3 * b3_z * t*t;
cartXDotDot[1][4].vel[0] = 2 * b2_x + 6 * b3_x*t;
cartXDotDot[1][4].vel[1] = 2 * b2_y + 6 * b3_y*t;
cartXDotDot[1][4].vel[2] = 2 * b2_z + 6 * b3_z*t;
#ifdef DESIRED_CARTESIAN_VALUES
printf("%f %f %f %f %f %f %f\n", t, cartX[1][4].p[0], cartX[1][4].p[1], cartX[1][4].p[2], cartXDot[1][4].vel[0], cartXDot[1][4].vel[1], cartXDot[1][4].vel[2]);
#endif //~DESIRED_CARTESIAN_VALUES
jointPoses[1](0) = a0_q0 + a1_q0 * t + a2_q0 * t * t + a3_q0 * t * t*t;
jointPoses[1](1) = a0_q1 + a1_q1 * t + a2_q1 * t * t + a3_q1 * t * t*t;
jointPoses[1](2) = a0_q2 + a1_q2 * t + a2_q2 * t * t + a3_q2 * t * t*t;
jointPoses[1](3) = a0_q3 + a1_q3 * t + a2_q3 * t * t + a3_q3 * t * t*t;
jointPoses[1](4) = a0_q4 + a1_q4 * t + a2_q4 * t * t + a3_q4 * t * t*t;
jointRates[1](0) = a1_q0 + 2 * a2_q0 * t + 3 * a3_q0 * t*t;
jointRates[1](1) = a1_q1 + 2 * a2_q1 * t + 3 * a3_q1 * t*t;
jointRates[1](2) = a1_q2 + 2 * a2_q2 * t + 3 * a3_q2 * t*t;
jointRates[1](3) = a1_q3 + 2 * a2_q3 * t + 3 * a3_q3 * t*t;
jointRates[1](4) = a1_q4 + 2 * a2_q4 * t + 3 * a3_q4 * t*t;
jointAccelerations[1](0) = 2 * a2_q0 + 6 * a3_q0*t;
jointAccelerations[1](1) = 2 * a2_q1 + 6 * a3_q1*t;
jointAccelerations[1](2) = 2 * a2_q2 + 6 * a3_q2*t;
jointAccelerations[1](3) = 2 * a2_q3 + 6 * a3_q3*t;
jointAccelerations[1](4) = 2 * a2_q4 + 6 * a3_q4*t;
#ifdef DESIRED_JOINT_VALUES
printf("%f %f %f %f %f %f %f %f %f %f %f\n", t,jointPoses[1](0), jointPoses[1](1),jointPoses[1](2), jointPoses[1](3), jointPoses[1](4),
jointRates[1](0), jointRates[1](1), jointRates[1](2), jointRates[1](3), jointRates[1](4));
#endif //~DESIRED_JOINT_VALUES
status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, bethaControl, externalNetForces, jointTorques[0], jointControlTorques[0]);
constraintSolver.getLinkCartesianPose(cartX[0]);
constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
constraintSolver.getLinkCartesianAcceleration(cartXDotDot[0]);
cartX[0][4].M.GetEulerZYX(alfa,beta,gamma);
#ifdef ACTUAL_CARTESIAN_VALUES
printf("%f %f %f %f %f %f %f\n", t, cartX[0][4].p[0], cartX[0][4].p[1], cartX[0][4].p[2], alfa, beta, gamma);
#endif //~ACTUAL_CARTESIAN_VALUES
//Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
for (unsigned int i = 0; i < numberOfJoints; i++)
{
jointRates[0](i) = jointRates[0](i) + jointAccelerations[0](i) * timeDelta; //Euler Forward
jointPoses[0](i) = jointPoses[0](i) + (jointRates[0](i) - jointAccelerations[0](i) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
}
#ifdef ACTUAL_JOINT_VALUES
printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
t,jointPoses[0](0), jointPoses[0](1),jointPoses[0](2), jointPoses[0](3), jointPoses[0](4), jointRates[0](0), jointRates[0](1), jointRates[0](2), jointRates[0](3), jointRates[0](4),
jointAccelerations[0](0), jointAccelerations[0](1), jointAccelerations[0](2), jointAccelerations[0](3), jointAccelerations[0](4), jointTorques[0](0), jointTorques[0](1), jointTorques[0](2), jointTorques[0](3), jointTorques[0](4));
#endif //~ACTUAL_JOINT_VALUES
//Error
for (unsigned int i = 0; i < numberOfJoints; i++)
{
jointPoses[2](i) = jointPoses[1](i) - jointPoses[0](i);
jointRates[2](i) = jointRates[1](i) - jointRates[0](i);
jointAccelerations[2](i) = jointAccelerations[1](i) - jointAccelerations[0](i);
// jointPoses[3](i) += timeDelta * jointPoses[2](i);
}
#ifdef JOINT_ERROR_VALUES
printf("%f %f %f %f %f %f\n", t,jointPoses[2](0), jointPoses[2](1),jointPoses[2](2), jointPoses[2](3), jointPoses[2](4));
#endif //~JOINT_ERROR_VALUES
cartX[2][4].p[0] = cartX[1][4].p[0] - cartX[0][4].p[0];
cartX[2][4].p[1] = cartX[1][4].p[1] - cartX[0][4].p[1];
cartX[2][4].p[2] = cartX[1][4].p[2] - cartX[0][4].p[2];
cartXDot[2][4].vel[0] = cartXDot[1][4].vel[0] - cartXDot[0][4].vel[0];
cartXDot[2][4].vel[1] = cartXDot[1][4].vel[1] - cartXDot[0][4].vel[1];
cartXDot[2][4].vel[2] = cartXDot[1][4].vel[2] - cartXDot[0][4].vel[2];
cartXDotDot[2][4].vel[0] = cartXDotDot[1][4].vel[0] - cartXDotDot[0][4].vel[0];
cartXDotDot[2][4].vel[1] = cartXDotDot[1][4].vel[1] - cartXDotDot[0][4].vel[1];
cartXDotDot[2][4].vel[2] = cartXDotDot[1][4].vel[2] - cartXDotDot[0][4].vel[2];
cartX[3][4].p[0] += timeDelta * cartX[2][4].p[0]; //for integral term;
cartX[3][4].p[1] += timeDelta * cartX[2][4].p[1];
cartX[3][4].p[2] += timeDelta * cartX[2][4].p[2];
#ifdef CARTESIAN_ERROR_VALUES
printf("%f %f %f %f %f %f %f\n", t, cartX[2][4].p[0], cartX[2][4].p[1], cartX[2][4].p[2], cartXDot[2][4].vel[0], cartXDot[2][4].vel[1], cartXDot[2][4].vel[2]);
#endif //~CARTESIAN_ERROR_VALUES
//Regulator or controller
//acceleration energy control
betha(0) = alpha(0, 0)*(K * cartXDotDot[2][numberOfLinks-1].vel[0] + (Kv[0]) * cartXDot[2][numberOfLinks-1].vel[0] + (Kp[0]) * cartX[2][numberOfLinks-1].p[0]+ Ki[1] * cartX[3][numberOfLinks-1].p[0]);
betha(1) = alpha(1, 1)*(K * cartXDotDot[2][numberOfLinks-1].vel[1] + (Kv[1]) * cartXDot[2][numberOfLinks-1].vel[1] + (Kp[1]) * cartX[2][numberOfLinks-1].p[1] + Ki[1] * cartX[3][numberOfLinks-1].p[1]);
// priority posture control
// jointControlTorques[0](0) = jointAccelerations[1](0) + (Kpq[0])*jointPoses[2](0) + (Kvq[0])*jointRates[2](0);
// jointControlTorques[0](1) = jointAccelerations[1](1) + (Kpq[1])*jointPoses[2](1) + (Kvq[1])*jointRates[2](1);//computed joint torque control
// jointControlTorques[0](2) = jointAccelerations[1](2) + (Kpq[2])*jointPoses[2](2) + (Kvq[2])*jointRates[2](2);
// jointControlTorques[0](3) = jointAccelerations[1](3) + (Kpq[3])*jointPoses[2](3) + (Kvq[3])*jointRates[2](3);
// jointControlTorques[0](4) = jointAccelerations[1](4) + (Kpq[3])*jointPoses[2](4) + (Kvq[4])*jointRates[2](4);
//priority constraint control
// feedforwardJointTorque0 = jointAccelerations[1](0) + (Kpq[0])*jointPoses[2](0) + (Kvq[0])*jointRates[2](0);
feedforwardJointTorque1 = jointAccelerations[1](1) + (Kpq[1])*jointPoses[2](1) + (Kvq[1])*jointRates[2](1);
feedforwardJointTorque2 = jointAccelerations[1](2) + (Kpq[2])*jointPoses[2](2) + (Kvq[2])*jointRates[2](2);
feedforwardJointTorque3 = jointAccelerations[1](3) + (Kpq[3])*jointPoses[2](3) + (Kvq[3])*jointRates[2](3);
feedforwardJointTorque4 = jointAccelerations[1](4) + (Kpq[4])*jointPoses[2](4) + (Kvq[4])*jointRates[2](4);
// jointTorques[0](0) = jointTorques[0](0) + feedforwardJointTorque0;
jointTorques[0](1) = jointTorques[0](1) + feedforwardJointTorque1;
jointTorques[0](2) = jointTorques[0](2) + feedforwardJointTorque2;
jointTorques[0](3) = jointTorques[0](3) + feedforwardJointTorque3;
jointTorques[0](4) = jointTorques[0](4) + feedforwardJointTorque4;
//For cartesian space control one needs to calculate from the obtained joint space value, new cartesian space poses.
//Then based on the difference of the signal (desired-actual) we define a regulation function (controller)
// this difference should be compensated either by joint torques.
// externalNetForces[numberOfLinks-1].force[0] = ((Kv[0]) * cartXDot[2][numberOfLinks-1].vel[0] + (Kp[0]) * cartX[2][numberOfLinks-1].p[0] );
// externalNetForces[numberOfLinks-1].force[1] = ((Kv[1]) * cartXDot[2][numberOfLinks-1].vel[1] + (Kp[1]) * cartX[2][numberOfLinks-1].p[1] );
externalNetForces[numberOfLinks-1].force[2] = ((Kv[2]) * cartXDot[2][numberOfLinks-1].vel[2] + (Kp[2]) * cartX[2][numberOfLinks-1].p[2] );
}
//~Definition of process main loop
//-------------------------------------------------------------------------------------//
return 0;
}
