FUNCTION VOID z_exit
(
FUNINT sfi, /* in: value for $sfi */
TEXT *skey /* in: value for $sky */
)
{
#define ALDIM(bytes) 1+((bytes-1)/sizeof (ALIGN))
#define HEAD_SIZ (sizeof(struct PARBLK) - P_BYTES + 8) /* 8 for align safety*/
#define ZBLKDIM (3*sizeof(struct VARIABLE) + 2*STRINGSIZ + HEAD_SIZ)
/* block dimension */
IMPORT TEXT savekey[]; /* key for last message */
TEXT *keyptr[1]; /* pointer to key string */
TAEINT sfival[1];
ALIGN block[ALDIM(ZBLKDIM)]; /* parameter block to send */
struct PARBLK *termblk; /* pointer to parameter block*/
CODE code;
keyptr[0] = skey; /* assume key present */
if (NULLSTR(skey))
keyptr[0] = savekey; /* else, give old key */
sfival[0] = sfi;
termblk = (struct PARBLK *)block; /* cast pointer */
q_init(termblk, ZBLKDIM - HEAD_SIZ, P_ABORT); /* initialize the block*/
q_intg(termblk, "$SFI", 1, sfival, P_ADD); /* put sfi in block */
q_string(termblk, "$SKEY", 1, keyptr, P_ADD); /* put skey in block */
code = q_out(termblk); /* send block to tm */
procexit(code);
return;
}
// ----------------------------------------------------------------------------------------------
Eigen::MatrixXd JacoIKSolver::desiredAngles(geometry_msgs::Pose p_in, JacoAngles JAngle)
{
Eigen::MatrixXd JAngle_out = Eigen::MatrixXd::Zero(1,6);
KDL::JntArray q_out, joint_in;
KDL::Frame f_in;
tf::PoseMsgToKDL(p_in,f_in);
joint_in.resize(6);
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;
ik_solver_->CartToJnt(joint_in, f_in, q_out);
for (int i = 0; i < 6; i++){
JAngle_out(0,i)= saturate(q_out(i) * RTD) ;
}
return JAngle_out;
}
JacoAngles JacoIKSolver::cartesianToJoints(geometry_msgs::Pose p_in, JacoAngles JAngle)
{
JacoAngles JAngle_out;
KDL::JntArray q_out, joint_in;
KDL::Frame f_in;
tf::PoseMsgToKDL(p_in,f_in);
joint_in.resize(6);
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;
ik_solver_->CartToJnt(joint_in, f_in, q_out);
JAngle_out.Actuator1 = q_out(0) * RTD + 180;
JAngle_out.Actuator2 = q_out(1) * RTD + 270;
JAngle_out.Actuator3 = q_out(2) * RTD + 90;
JAngle_out.Actuator4 = q_out(3) * RTD + 180;
JAngle_out.Actuator5 = q_out(4) * RTD + 180;
JAngle_out.Actuator6 = q_out(5) * RTD + 270;
return JAngle_out;
}
int zvq_out(void *parblk)
{
int status = 0;
#if RTL_USE_TAE && RTL_USE_SHELL_VIC
if (in_vicar)
status = q_out(parblk); /* TAE version */
else
status = zzq_out(parblk); /* Shell-VICAR version */
#else
#if RTL_USE_TAE
status = q_out(parblk);
#else
#if RTL_USE_SHELL_VIC
status = zzq_out(parblk);
#else
status = 0; /* neither? */
#endif /* shvic only */
#endif /* tae only */
#endif /* both */
return status;
}
TEST(McmcSample, cont_params_by_vector) {
Eigen::VectorXd q(2);
q(0) = 5;
q(1) = 1;
double log_prob = -10;
double accept_stat = 0.5;
stan::mcmc::sample s(q, log_prob, accept_stat);
const Eigen::VectorXd& q_out = s.cont_params();
for (int i = 0; i < s.size_cont(); ++i)
EXPECT_EQ(q(i), q_out(i));
}
/**
* @brief Send attitude setpoint to FCU attitude controller
*
* @note ENU frame.
*/
void send_attitude_target(const ros::Time &stamp, const Eigen::Affine3d &tr) {
/* Thrust + RPY, also bits numbering started from 1 in docs
*/
const uint8_t ignore_all_except_q = (1 << 6) | (7 << 0);
float q[4];
// Eigen use same convention as mavlink: w x y z
Eigen::Map<Eigen::Quaternionf> q_out(q);
q_out = UAS::transform_frame_enu_ned(Eigen::Quaterniond(tr.rotation())).cast<float>();
set_attitude_target(stamp.toNSec() / 1000000,
ignore_all_except_q,
q,
0.0, 0.0, 0.0,
0.0);
}
bool unigripper_motoman_plant_t::IK_solver(const config_t& effector_config, space_point_t* computed_state, bool set_grasping, const space_point_t* seed_state, bool do_min)
{
double qx,qy,qz,qw;
double x,y,z;
effector_config.get_orientation().get(qx,qy,qz,qw);
effector_config.get_position(x,y,z);
KDL::Chain chain1;
my_tree->getChain("base_link","vacuum_volume",chain1);
KDL::JntArray q(chain1.getNrOfJoints());
KDL::JntArray q_out(chain1.getNrOfJoints());
KDL::JntArray q_min(chain1.getNrOfJoints());
KDL::JntArray q_max(chain1.getNrOfJoints());
std::vector< double > state_var;
if( seed_state != NULL )
state_space->copy_point_to_vector( seed_state, state_var );
else
state_space->copy_to_vector( state_var );
q(0) = state_var[0];
q(1) = state_var[1];
q(2) = state_var[2];
q(3) = state_var[3];
q(4) = state_var[4];
q(5) = state_var[5];
q(6) = state_var[6];
q(7) = state_var[7];
q(8) = state_var[15];
q_min(0) = state_space->get_bounds()[0]->get_lower_bound();
q_min(1) = state_space->get_bounds()[1]->get_lower_bound();
q_min(2) = state_space->get_bounds()[2]->get_lower_bound();
q_min(3) = state_space->get_bounds()[3]->get_lower_bound();
q_min(4) = state_space->get_bounds()[4]->get_lower_bound();
q_min(5) = state_space->get_bounds()[5]->get_lower_bound();
q_min(6) = state_space->get_bounds()[6]->get_lower_bound();
q_min(7) = state_space->get_bounds()[7]->get_lower_bound();
q_min(8) = state_space->get_bounds()[15]->get_lower_bound();
q_max(0) = state_space->get_bounds()[0]->get_upper_bound();
q_max(1) = state_space->get_bounds()[1]->get_upper_bound();
q_max(2) = state_space->get_bounds()[2]->get_upper_bound();
q_max(3) = state_space->get_bounds()[3]->get_upper_bound();
q_max(4) = state_space->get_bounds()[4]->get_upper_bound();
q_max(5) = state_space->get_bounds()[5]->get_upper_bound();
q_max(6) = state_space->get_bounds()[6]->get_upper_bound();
q_max(7) = state_space->get_bounds()[7]->get_upper_bound();
q_max(8) = state_space->get_bounds()[15]->get_upper_bound();
KDL::ChainFkSolverPos_recursive fk_solver(chain1);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain1);
KDL::ChainIkSolverPos_NR_JL ik_solver(chain1,q_min,q_max,fk_solver,ik_solver_vel,1000,1e-6);
KDL::Frame F(KDL::Rotation::Quaternion(qx,qy,qz,qw),KDL::Vector(x,y,z));
bool ik_res = (ik_solver.CartToJnt(q,F,q_out)>=0);
if(ik_res)
{
std::vector<double> state_vec;
state_vec = state_var;
state_vec[0] = q_out(0);
state_vec[1] = q_out(1);
state_vec[2] = q_out(2);
state_vec[3] = q_out(3);
state_vec[4] = q_out(4);
state_vec[5] = q_out(5);
state_vec[6] = q_out(6);
state_vec[7] = q_out(7);
state_vec[15] = q_out(8);
state_vec[16] = set_grasping ? (double)GRIPPER_CLOSED : GRIPPER_OPEN;
state_space->copy_vector_to_point( state_vec, computed_state );
}
return ik_res;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "leg_kinematics");
KDL::Tree tree;
KDL::Chain chain;
ros::NodeHandle node;
KDL::ChainFkSolverPos_recursive* fk_solver;
KDL::HP_ChainIkSolverPos_NR_JL *ik_solver_pos;
KDL::ChainIkSolverVel_pinv* ik_solver_vel;
std::string robot_desc_string;
node.param("robot_description", robot_desc_string, std::string("robot_description"));
if (!kdl_parser::treeFromString(robot_desc_string, tree)) {
ROS_ERROR("Failed to construct kdl tree");
return false;
}
if (!tree.getChain("base_link", "tibia_foot_r1", chain)) {
ROS_ERROR("Failed to construct kdl chain");
return false;
}
ROS_INFO("Construct kdl chain");
// unsigned int nj = chain.getNrOfJoints();
// unsigned int js = chain.getNrOfSegments();
// std_msgs::String msg;
// std::stringstream ss;
// ss << "# J: " << nj << " # S: " << js;
// msg.data = ss.str();
// ROS_INFO("Construct kdl tree");
// ROS_INFO("%s", msg.data.c_str()) ;
fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
// Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);
// matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;
// ik_solver_vel -> setWeightTS(matrix_Mx);
KDL::JntArray joint_min(chain.getNrOfJoints());
KDL::JntArray joint_max(chain.getNrOfJoints());
joint_min(0) = -1.57;
joint_min(1) = -1.57;
joint_min(2) = -1.57;
joint_max (0) = 1.57;
joint_max (1) = 1.57;
joint_max (2) = 1.57;
ik_solver_pos = new KDL::HP_ChainIkSolverPos_NR_JL (chain, joint_min, joint_max,
*fk_solver, *ik_solver_vel, 20, 0.0001);
KDL::JntArray q_init(chain.getNrOfJoints());
q_init (0) = 0;
q_init (1) = 0;
q_init (2) = 0;
KDL::JntArray q_out(chain.getNrOfJoints());
// KDL::Segment my_seg = chain.getSegment(3);
// KDL::Frame my_fr;
//
// if (fk_solver.JntToCart(q_init, my_fr) >= 0){
//
// std_msgs::String msg;
// std::stringstream ss;
// ss << "# 0: " << my_fr.p[0] << " # 1: " << my_fr.p[1] << " # 2: " << my_fr.p[2];
// msg.data = ss.str();
// ROS_INFO("Construct kdl tree");
// ROS_INFO("%s", msg.data.c_str()) ;
// }
double x = 0.16509, y = -0.18567, z = 0.053603; //0.16509; -0.18567; 0.053603
KDL::Frame p_in (KDL::Vector(0.12291, -0.085841, -0.16766));
int ik_valid = ik_solver_pos -> CartToJnt(q_init, p_in, q_out);
if (ik_valid >= 0) {
ROS_INFO("\nJoint 1: %f\nJoint 2: %f\nJoint 3: %f\n", q_out(0),q_out(1),q_out(2));
}
else {
ROS_ERROR("IK not found");
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "leg_kinematics_pub");
ros::NodeHandle node;
KDL::Tree tree;
KDL::Chain chain;
KDL::ChainFkSolverPos_recursive* fk_solver;
KDL::HP_ChainIkSolverPos_NR_JL* ik_solver_pos;
KDL::ChainIkSolverVel_pinv* ik_solver_vel;
ros::Publisher leg_msg_pub = node.advertise<crab_msgs::LegJointsState>("leg_states", 1);
ros::Publisher joint_msg_pub = node.advertise<sensor_msgs::JointState>("leg_joints_states", 1);
ros::Rate loop_rate(30);
std::string robot_desc_string;
node.param("robot_description", robot_desc_string, std::string("robot_description"));
if (!kdl_parser::treeFromString(robot_desc_string, tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
if (!tree.getChain("base_link", "tibia_foot_r1", chain)) {
ROS_ERROR("Failed to construct kdl chain");
return false;
}
ROS_INFO("Construct kdl chain");
fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
// Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);
// matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;
// ik_solver_vel -> setWeightTS(matrix_Mx);
KDL::JntArray joint_min(chain.getNrOfJoints());
KDL::JntArray joint_max(chain.getNrOfJoints());
joint_min(0) = -1.57; joint_min(1) = -1.57; joint_min(2) = -1.57;
joint_max (0) = 1.57; joint_max (1) = 1.57; joint_max (2) = 1.57;
ik_solver_pos = new KDL::HP_ChainIkSolverPos_NR_JL (chain, joint_min, joint_max,
*fk_solver, *ik_solver_vel, 100, 0.0001);
KDL::JntArray q_init(chain.getNrOfJoints());
q_init (0) = 0; q_init (1) = 0; q_init (2) = 0;
KDL::JntArray q_out(chain.getNrOfJoints());
crab_msgs::LegJointsState leg_msg;
sensor_msgs::JointState joint_msg;
int ik_valid;
double x_0 = 0.14208, y_0 = -0.14555, z_0 = -0.11245, x, y, fi; //0.14208; -0.14555; -0.11145
while (ros::ok()){
if (fi > 6.28) fi = 0;
x = x_0 + 0.05 * cos(fi);
y = y_0 + 0.05 * sin(fi);
KDL::Frame p_in (KDL::Vector(x, y, z_0));
ik_valid = ik_solver_pos -> CartToJnt(q_init, p_in, q_out);
if (ik_valid >= 0){
ROS_INFO("\nJoint 1: %f\nJoint 2: %f\nJoint 3: %f\n", q_out(0),q_out(1),q_out(2));
for (int i=0; i<3; i++){
leg_msg.joint[i] = q_out(i);
}
leg_msg_pub.publish(leg_msg);
joint_msg.name.push_back("coxa_joint_r1");
joint_msg.position.push_back(q_out(0));
joint_msg.name.push_back("femur_joint_r1");
joint_msg.position.push_back(q_out(1));
joint_msg.name.push_back("tibia_joint_r1");
joint_msg.position.push_back(q_out(2));
joint_msg_pub.publish(joint_msg);
joint_msg.name.clear();
joint_msg.position.clear();
}
else {
ROS_ERROR("IK not found");
}
fi += 0.05;
q_init = q_out;
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
