void ExcavaROBOdometry::update()
{
if (!isInputValid()) {
if (verbose_)
debug_publisher_->msg_.input_valid = false;
ROS_DEBUG("Odometry: Input velocities are invalid");
return;
}
else {
if (verbose_)
debug_publisher_->msg_.input_valid = true;
}
current_time_ = base_kin_.robot_state_->getTime();
ros::Time update_start = ros::Time::now();
updateOdometry();
// double update_time = (ros::Time::now()-update_start).toSec();
ros::Time publish_start = ros::Time::now();
if (publish_odom_)
publishOdometry();
if (publish_odometer_)
publishOdometer();
if (publish_state_odometry_)
publishStateOdometry();
if (publish_tf_)
publishTransform();
last_r_wheel_rotation_ = current_r_wheel_rotation_;
last_l_wheel_rotation_ = current_l_wheel_rotation_;
last_time_ = current_time_;
sequence_ ++;
}
int visualiseQueryDensityFunction(tf::TransformBroadcaster &br, MarkerPublisher &markers_pub, int m_id, const QueryDensity &qd, const std::string &link_name, const KDL::Frame &T_L_C, const KDL::Frame &T_W_O, const std::string &frame_id) {
// visualisation of query density
// KDL::Rotation rot(KDL::Rotation::RotZ(-(-90.0+30.0)/180.0*PI));
KDL::Rotation rot(KDL::Rotation::RotY((-90.0+0.0)/180.0*PI) * KDL::Rotation::RotZ((80.0+100.0)/180.0*PI));
double grid_size = 0.005;
std::list<std::pair<KDL::Frame, double> > test_list;
double max_cost = 0.0;
for (double x = -0.15; x < 0.35; x += grid_size) {
for (double y = -0.22; y < 0.22; y += grid_size) {
KDL::Frame T_O_L = KDL::Frame(rot, KDL::Vector(0, y, x));
double cost = qd.getQueryDensity(link_name, T_O_L * T_L_C);
test_list.push_back(std::make_pair(T_O_L, cost));
if (cost > max_cost) {
max_cost = cost;
}
}
}
std::cout << "max_cost " << max_cost << std::endl;
for (std::list<std::pair<KDL::Frame, double> >::const_iterator it = test_list.begin(); it != test_list.end(); it++) {
double color = it->second / max_cost;
m_id = markers_pub.addSinglePointMarkerCube(m_id, it->first * KDL::Vector(), color, color, color, 1, grid_size, grid_size, grid_size, frame_id);
}
markers_pub.publish();
for (int i = 0; i < 100; i++) {
publishTransform(br, T_W_O * KDL::Frame(rot, KDL::Vector(0,0,-0.2)), link_name, "world");
ros::spinOnce();
ros::Duration(0.001).sleep();
}
return m_id;
}
void SlamGMapping::publishLoop(double transform_publish_period){
if(transform_publish_period == 0)
return;
ros::Rate r(1.0 / transform_publish_period);
while(ros::ok()){
publishTransform();
r.sleep();
}
}
void ImuFilter::imuMagCallback(
const ImuMsg::ConstPtr& imu_msg_raw,
const MagMsg::ConstPtr& mag_msg)
{
boost::mutex::scoped_lock(mutex_);
const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
const geometry_msgs::Vector3& lin_acc = imu_msg_raw->linear_acceleration;
const geometry_msgs::Vector3& mag_fld = mag_msg->vector;
ros::Time time = imu_msg_raw->header.stamp;
imu_frame_ = imu_msg_raw->header.frame_id;
if (!initialized_)
{
// initialize roll/pitch orientation from acc. vector
double sign = copysignf(1.0, lin_acc.z);
double roll = atan2(lin_acc.y, sign * sqrt(lin_acc.x*lin_acc.x + lin_acc.z*lin_acc.z));
double pitch = -atan2(lin_acc.x, sqrt(lin_acc.y*lin_acc.y + lin_acc.z*lin_acc.z));
double yaw = 0.0; // TODO: initialize from magnetic raeding?
tf::Quaternion init_q = tf::createQuaternionFromRPY(roll, pitch, yaw);
q1 = init_q.getX();
q2 = init_q.getY();
q3 = init_q.getZ();
q0 = init_q.getW();
w_bx_ = 0;
w_by_ = 0;
w_bz_ = 0;
last_time_ = time;
initialized_ = true;
}
// determine dt: either constant, or from IMU timestamp
float dt;
if (constant_dt_ > 0.0)
dt = constant_dt_;
else
dt = (time - last_time_).toSec();
last_time_ = time;
madgwickAHRSupdate(
ang_vel.x, ang_vel.y, ang_vel.z,
lin_acc.x, lin_acc.y, lin_acc.z,
mag_fld.x, mag_fld.y, mag_fld.z,
dt);
publishFilteredMsg(imu_msg_raw);
if (publish_tf_)
publishTransform(imu_msg_raw);
}
void sptam::sptam_node::publishTransformLoop()
{
if ( transform_publish_freq_ == 0 )
return;
ros::Rate r( transform_publish_freq_ );
while ( ros::ok() ) {
publishTransform();
r.sleep();
}
}
// Set and publish full state.
void Publisher::publishFullStateAsCallback(
const okvis::Time & t, const okvis::kinematics::Transformation & T_WS,
const Eigen::Matrix<double, 9, 1> & speedAndBiases,
const Eigen::Matrix<double, 3, 1> & omega_S)
{
setTime(t);
setOdometry(T_WS, speedAndBiases, omega_S); // TODO: provide setters for this hack
setPath(T_WS);
publishOdometry();
publishTransform();
publishPath();
}
int main(int argc, char **argv)
{
//ROS declaration
ros::init(argc, argv, "motorGo");
ros::NodeHandle my_node;
ros::Rate loop_rate(10);
ros::NodeHandle priv_node("~");
Robot platform(priv_node);
Scribe scribe(my_node, "cmd_vel", platform);
Poete poete(my_node, "odom");
//Lifter lift(my_node, priv_node);
Filter filt;
tf::TransformBroadcaster odom_broadcaster;
nav_msgs::Odometry nav;
int flag=0;
//Creation de Platform et test de verbose option
if ( argc > 1 ) {// argc should be 2 for correct execution
for(int i =0;i<argc;i++&&flag==0){
if (strcmp(argv[i],"--verbose")&&flag==0){
std::cout<<"VERBOSE MODE ON"<<std::endl;
scribe.setVerbose();
poete.setVerbose();
platform.setVerbose();
flag=1;
}
}
}
while(ros::ok()){
platform.odometry();
//if we read correctly
if(platform.getControl().getReadState()){
nav=platform.getOdom();
if(platform.getControl().getReadState()==true){
filt.add(nav);
nav=filt.filtering();
poete.publish(nav);
publishTransform(nav, odom_broadcaster);
}
}
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mcl_node");
ros::NodeHandle n("/mcl_node");;
n.param<std::string>("map_frame", mapFrame, "map");
n.param<std::string>("robot_base_link", baseFrame, "base_link");
n.param<double>("map_offset_x", coords.x0, 0.0);
n.param<double>("map_offset_y", coords.y0, 0.0);
n.param<int>("map_occupied_min_threshold", minOccupied, 10);
double odom_a1, odom_a2, odom_a3, odom_a4;
n.param<std::string>("odometry_frame", odomFrame, "odom");
n.param<double>("odometry_model_a1", odom_a1, 0.05);
n.param<double>("odometry_model_a2", odom_a2, 0.01);
n.param<double>("odometry_model_a3", odom_a3, 0.02);
n.param<double>("odometry_model_a4", odom_a4, 0.01);
//TODO: setup these IR constants more properly.
double irZhit, irZrm;
n.param<double>("ir_model_sigma_hit", irSigma, 0.1);
n.param<double>("ir_model_z_hit", irZhit, 0.95);
n.param<double>("ir_model_z_random_over_z_max", irZrm, 0.05);
int nParticles, mcl_rate;
double minDelta, aslow, afast;
double crashRadius, crashYaw, stdXY, stdYaw, locStdXY, locStdYaw;
n.param<int>("mcl_particles", nParticles, 200);
n.param<int>("mcl_rate", mcl_rate, 5);
n.param<double>("mcl_init_cone_radius", initConeRadius, 0.2);
n.param<double>("mcl_init_yaw_variance", initYawVar, 0.3);
n.param<double>("mcl_min_position_delta", minDelta, 0.001);
n.param<double>("mcl_aslow", aslow, 0.01);
n.param<double>("mcl_afast", afast, 0.2);
n.param<double>("mcl_crash_radius", crashRadius, 0.1);
n.param<double>("mcl_crash_yaw", crashYaw, 0.2);
n.param<double>("mcl_good_std_xy", stdXY, 0.05);
n.param<double>("mcl_good_std_yaw", stdYaw, 0.6);
n.param<double>("mcl_localized_std_xy", locStdXY, 0.05);
n.param<double>("mcl_localized_std_yaw", locStdYaw, 0.6);
tf::TransformBroadcaster broadcaster_obj;
tf_broadcaster = &broadcaster_obj;
tf::TransformListener listener_obj;
tf_listener = &listener_obj;
odom2map = new tf::Stamped<tf::Pose>;
message_filters::Subscriber<nav_msgs::Odometry> odom_sub(n, "/odom", 1);
message_filters::Subscriber<ir_sensors::RangeArray> range_sub(n,"/ir_publish/sensors", 1);
message_filters::Synchronizer<SyncPolicy> sync(SyncPolicy(10), odom_sub, range_sub);
sync.registerCallback(boost::bind(&odomRangeUpdate, _1, _2));
ros::Subscriber map_version_sub = n.subscribe<std_msgs::Int32>("/map_node/version", 2, mapVersionCB);
ros::Publisher particle_pub_obj = n.advertise<geometry_msgs::PoseArray>("/mcl/particles", 5);
ros::Publisher localized_pub = n.advertise<std_msgs::Bool>("/mcl/is_localized", 5);
geometry_msgs::PoseArray poseArray;
poseArray.header.frame_id = mapFrame;
//Wait 8 s for the map service.
if(!ros::service::waitForService("/map_node/get_map", 8000)) {
ROS_ERROR("Map service unreachable.");
return -1;
}
mapInflated = new nav_msgs::OccupancyGrid();
mapDistance = new nav_msgs::OccupancyGrid();
ros::ServiceClient map_client_obj = n.serviceClient<map_tools::GetMap>("/map_node/get_map");
map_client = &map_client_obj;
if(!updateMap()) {
return -1;
}
struct PoseState pose, goodStd, locStd;
goodStd.set(stdXY);
goodStd.yaw = stdYaw;
locStd.set(locStdXY);
locStd.yaw = locStdYaw;
pose.set(0.0);
pose.x += coords.x0;
pose.y += coords.y0;
om = new OdometryModel(odom_a1, odom_a2, odom_a3, odom_a4);
mclEnabled = false;
coords.x = coords.x0;
coords.y = coords.y0;
coords.th = 0.0;
mc = new MonteCarlo(om, &isPointFree, nParticles, minDelta,
aslow, afast, crashRadius, crashYaw, goodStd);
irm = new RangeModel(&getDist, irZhit, irZrm);
mc->addSensor(irm);
double csz = mapInflated->info.resolution;
double wc = ((double)mapInflated->info.width);
double hc = ((double)mapInflated->info.height);
assert(csz > 0.00001);
mc->init(pose, initConeRadius, initYawVar, wc*csz, hc*csz);
//mc->init(wc*csz, hc*csz);
mclEnabled = true;
current_time = ros::Time::now();
int rate = 40, counter = 0;
ros::Rate loop_rate(rate);
if(!updateMap())
return -1;
*odom2map = mclTransform();
struct PoseState odom0;
bool firstMcl = true;
double dx = 0, dy = 0, dyaw = 0, dx1 = 0, dy1 = 0, dyaw1 = 0;
std_msgs::Bool isLocalizedMsg;
isLocalizedMsg.data = isLocalized;
ros::Subscriber init_mcl_sub = n.subscribe<geometry_msgs::Pose>("/mcl/initial_pose", 2, initMcl);
while (ros::ok())
{
if(!firstMcl) {
dx = odomState.x - odom0.x;
dy = odomState.y - odom0.y;
dyaw = odomState.yaw - odom0.yaw;
dx1 = dx;
dy1 = dy;
dyaw1 = dyaw;
}
if(counter % (rate/mcl_rate) == 0) {
if(mclEnabled) {
if(firstMcl) {
odom0 = odomState;
firstMcl = false;
}
odom0 = odomState;
if(runMonteCarlo()) {
dx = 0;
dy = 0;
dyaw = 0;
}
particles2PoseArray(mc->getParticles(), poseArray);
particle_pub_obj.publish(poseArray);
struct PoseState std = mc->getStd();
if(std.x > locStd.x || std.y > locStd.y || std.yaw > locStd.yaw) {
isLocalized = false;
} else {
isLocalized = true;
}
}
isLocalizedMsg.data = isLocalized;
localized_pub.publish(isLocalizedMsg);
counter = 0;
}
counter++;
if(!firstMcl) {
struct PoseState avg = mc->getState();
coords.x = avg.x + dx;
coords.y = avg.y + dy;
coords.th = avg.yaw + dyaw;
//Do not update transform when rotating quickly on spot.
if((std::sqrt(dx1*dx1 + dy1*dy1) > 0.03/(double)mcl_rate)
&& isLocalized)
*odom2map = mclTransform();
}
publishTransform(*odom2map);
ros::spinOnce();
loop_rate.sleep();
ros::Duration last_update = ros::Time::now() - current_time;
if(last_update > ros::Duration(1.2/(double)rate))
current_time = ros::Time::now();
}
return 0;
}
void processKinect(KinectController &kinect_controller)
{
XnUserID users[3];
XnUInt16 users_count = 3;
xn::UserGenerator& UserGenerator = kinect_controller.getUserGenerator();
xn::DepthGenerator& depthGenerator = kinect_controller.getDepthGenerator();
UserGenerator.GetUsers(users, users_count);
skeleton_markers::Skeleton g_skel;
for (int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
if (!UserGenerator.GetSkeletonCap().IsTracking(user)){
continue;
}
XnPoint3D com;
UserGenerator.GetCoM(user, com);
depthGenerator.ConvertRealWorldToProjective(1, &com, &com);
glVertex2f(com.X, com.Y);
// Calculate the distance between human and camera
float dist = com.Z /1000.0f;
geometry_msgs::Twist vel;
if(g_LeftHandPositionHistory.Speed()>150)
{
vel.linear.x = 0.00;
vel.angular.z = -0.08;
ROS_INFO("Robot is moving anticlockwise %.2f", g_LeftHandPositionHistory.Speed());
}
else if(g_RightHandPositionHistory.Speed()>150 && g_LeftHandPositionHistory.Speed()>150)
{
ROS_INFO("Robot is moving....");
ros::Duration(1.0).sleep(); // sleep for 1 second
ROS_INFO("Robot.....");
}
else if(g_RightHandPositionHistory.Speed()>150){
vel.linear.x = 0.00;
vel.angular.z = 0.08;
ROS_INFO("Robot is moving clockwise");
}
else{
//velocity command send to robot for follow human.
if(dist >=1.5)
{
ROS_INFO("I am following you");
vel.linear.x = 0.1;
vel.angular.z = 0.0;
}
else if(dist <=1.5 && dist >=1.0)
{
ROS_INFO("I am rounding ...");
vel.linear.x = 0.0;
vel.angular.z = 0.1;
}
else{
ROS_INFO(" You are too near to me");
vel.linear.x = -0.1;
vel.angular.z = 0.0;
}
}
handtrajectory(UserGenerator, depthGenerator, user, XN_SKEL_RIGHT_HAND, true);
handtrajectory(UserGenerator, depthGenerator, user, XN_SKEL_LEFT_HAND, true);
publishTransform(kinect_controller, user, XN_SKEL_HEAD, fixed_frame, "head", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_NECK, fixed_frame, "neck", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_TORSO, fixed_frame, "torso", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_SHOULDER, fixed_frame, "left_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_ELBOW, fixed_frame, "left_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HAND, fixed_frame, "left_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_SHOULDER, fixed_frame, "right_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_ELBOW, fixed_frame, "right_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HAND, fixed_frame, "right_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HIP, fixed_frame, "left_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_KNEE, fixed_frame, "left_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_FOOT, fixed_frame, "left_foot", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HIP, fixed_frame, "right_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_KNEE, fixed_frame, "right_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_FOOT, fixed_frame, "right_foot", g_skel);
// Input Joint Positions And Orientations from kinect
// Upper Joints positions
XnSkeletonJointPosition joint_position_head;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_HEAD, joint_position_head);
KDL::Vector head(joint_position_head.position.X, joint_position_head.position.Y, joint_position_head.position.Z);
XnSkeletonJointPosition joint_position_neck;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_NECK, joint_position_neck);
KDL::Vector neck(joint_position_neck.position.X, joint_position_neck.position.Y, joint_position_neck.position.Z);
XnSkeletonJointPosition joint_position_torso;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_TORSO, joint_position_torso);
KDL::Vector torso(joint_position_torso.position.X, joint_position_torso.position.Y, joint_position_torso.position.Z);
// Left Arm ****
XnSkeletonJointPosition joint_position_left_hand;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HAND, joint_position_left_hand);
KDL::Vector left_hand(joint_position_left_hand.position.X, joint_position_left_hand.position.Y, joint_position_left_hand.position.Z);
XnSkeletonJointPosition joint_position_left_elbow;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_ELBOW, joint_position_left_elbow);
KDL::Vector left_elbow(joint_position_left_elbow.position.X, joint_position_left_elbow.position.Y, joint_position_left_elbow.position.Z);
XnSkeletonJointPosition joint_position_left_shoulder;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, joint_position_left_shoulder);
KDL::Vector left_shoulder(joint_position_left_shoulder.position.X, joint_position_left_shoulder.position.Y, joint_position_left_shoulder.position.Z);
// Right Arm ****
XnSkeletonJointPosition joint_position_right_hand;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_HAND, joint_position_right_hand);
KDL::Vector right_hand(joint_position_right_hand.position.X, joint_position_right_hand.position.Y, joint_position_right_hand.position.Z);
XnSkeletonJointPosition joint_position_right_elbow;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_ELBOW, joint_position_right_elbow);
KDL::Vector right_elbow(joint_position_right_elbow.position.X, joint_position_right_elbow.position.Y, joint_position_right_elbow.position.Z);
XnSkeletonJointPosition joint_position_right_shoulder;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_SHOULDER, joint_position_right_shoulder);
KDL::Vector right_shoulder(joint_position_right_shoulder.position.X, joint_position_right_shoulder.position.Y, joint_position_right_shoulder.position.Z);
// Right Leg ****
XnSkeletonJointPosition joint_position_right_hip;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_HIP, joint_position_right_hip);
KDL::Vector right_hip(joint_position_right_hip.position.X, joint_position_right_hip.position.Y, joint_position_right_hip.position.Z);
XnSkeletonJointPosition joint_position_right_knee;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_KNEE, joint_position_right_knee);
KDL::Vector right_knee(joint_position_right_knee.position.X, joint_position_right_knee.position.Y, joint_position_right_knee.position.Z);
XnSkeletonJointPosition joint_position_right_foot;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_FOOT, joint_position_right_foot);
KDL::Vector right_foot(joint_position_right_foot.position.X, joint_position_right_foot.position.Y, joint_position_right_foot.position.Z);
// Left Leg ****
XnSkeletonJointPosition joint_position_left_hip;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HIP, joint_position_left_hip);
KDL::Vector left_hip(joint_position_left_hip.position.X, joint_position_left_hip.position.Y, joint_position_left_hip.position.Z);
XnSkeletonJointPosition joint_position_left_knee;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_KNEE, joint_position_left_knee);
KDL::Vector left_knee(joint_position_left_knee.position.X, joint_position_left_knee.position.Y, joint_position_left_knee.position.Z);
XnSkeletonJointPosition joint_position_left_foot;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_FOOT, joint_position_left_foot);
KDL::Vector left_foot(joint_position_left_foot.position.X, joint_position_left_foot.position.Y, joint_position_left_foot.position.Z);
// Upper joints rotations
XnSkeletonJointOrientation joint_orientation_head;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_HEAD, joint_orientation_head);
XnFloat* h = joint_orientation_head.orientation.elements;
KDL::Rotation head_rotation(h[0], h[1], h[2],
h[3], h[4], h[5],
h[6], h[7], h[8]);
double head_roll, head_pitch, head_yaw;
head_rotation.GetRPY(head_roll, head_pitch, head_yaw);
// head yaw
static double head_angle_yaw = 0;
if (joint_orientation_head.fConfidence >= 0.5)
{
head_angle_yaw = head_pitch;
}
// head pitch
static double head_angle_pitch = 0;
if (joint_orientation_head.fConfidence >= 0.5)
{
head_angle_pitch = head_roll;
}
XnSkeletonJointOrientation joint_orientation_neck;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_NECK, joint_orientation_neck);
XnFloat* n = joint_orientation_neck.orientation.elements;
KDL::Rotation neck_rotation(n[0], n[1], n[2],
n[3], n[4], n[5],
n[6], n[7], n[8]);
double neck_roll, neck_pitch, neck_yaw;
neck_rotation.GetRPY(neck_roll, neck_pitch, neck_yaw);
XnSkeletonJointOrientation joint_orientation_torso;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_TORSO, joint_orientation_torso);
XnFloat* t = joint_orientation_torso.orientation.elements;
KDL::Rotation torso_rotation(t[0], t[1], t[2],
t[3], t[4], t[5],
t[6], t[7], t[8]);
double torso_roll, torso_pitch, torso_yaw;
torso_rotation.GetRPY(torso_roll, torso_pitch, torso_yaw);
// torso yaw
static double torso_angle_yaw = 0;
if (joint_orientation_torso.fConfidence >= 0.5)
{
torso_angle_yaw = torso_pitch;
}
// the kinect and robot are in different rotation spaces,
// Process and output joint rotations to the robot.
// ARMS for robot
// left elbow roll ****
KDL::Vector left_elbow_hand(left_hand - left_elbow);
KDL::Vector left_elbow_shoulder(left_shoulder - left_elbow);
left_elbow_hand.Normalize();
left_elbow_shoulder.Normalize();
static double left_elbow_angle_roll = 0;
if (joint_position_left_hand.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5 &&
joint_position_left_shoulder.fConfidence >= 0.5)
{
left_elbow_angle_roll = acos(KDL::dot(left_elbow_hand, left_elbow_shoulder));
left_elbow_angle_roll = left_elbow_angle_roll - PI;
}
// right elbow roll ****
KDL::Vector right_elbow_hand(right_hand - right_elbow);
KDL::Vector right_elbow_shoulder(right_shoulder - right_elbow);
right_elbow_hand.Normalize();
right_elbow_shoulder.Normalize();
static double right_elbow_angle_roll = 0;
if (joint_position_right_hand.fConfidence >= 0.5 &&
joint_position_right_elbow.fConfidence >= 0.5 &&
joint_position_right_shoulder.fConfidence >= 0.5)
{
right_elbow_angle_roll = acos(KDL::dot(right_elbow_hand, right_elbow_shoulder));
right_elbow_angle_roll = -(right_elbow_angle_roll - PI);
}
// left shoulder roll ****
KDL::Vector left_shoulder_elbow(left_elbow - left_shoulder);
KDL::Vector left_shoulder_right_shoulder(right_shoulder - left_shoulder);
left_shoulder_elbow.Normalize();
left_shoulder_right_shoulder.Normalize();
static double left_shoulder_angle_roll = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5 &&
joint_position_left_shoulder.fConfidence >= 0.5)
{
left_shoulder_angle_roll = acos(KDL::dot(left_shoulder_elbow, left_shoulder_right_shoulder));
left_shoulder_angle_roll = left_shoulder_angle_roll - HALFPI;
}
// right shoulder roll ****
KDL::Vector right_shoulder_elbow(right_elbow - right_shoulder);
KDL::Vector right_shoulder_left_shoulder(left_shoulder - right_shoulder);
right_shoulder_elbow.Normalize();
right_shoulder_left_shoulder.Normalize();
static double right_shoulder_angle_roll = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5 &&
joint_position_right_elbow.fConfidence >= 0.5 &&
joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_roll = acos(KDL::dot(right_shoulder_elbow, right_shoulder_left_shoulder));
right_shoulder_angle_roll = -(right_shoulder_angle_roll - HALFPI);
}
// left shoulder pitch ****
static double left_shoulder_angle_pitch = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5)
{
left_shoulder_angle_pitch = asin(left_shoulder_elbow.y());
left_shoulder_angle_pitch = left_shoulder_angle_pitch + HALFPI;
}
// right shoulder pitch ****
static double right_shoulder_angle_pitch = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_pitch = asin(right_shoulder_elbow.y());
right_shoulder_angle_pitch = -(right_shoulder_angle_pitch + HALFPI);
}
// left shoulder yaw ****
static double left_shoulder_angle_yaw = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5)
{
left_shoulder_angle_yaw = asin(left_elbow_hand.x());
}
// right shoulder yaw ****
static double right_shoulder_angle_yaw = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_yaw = asin(right_elbow_hand.x());
right_shoulder_angle_yaw = -(right_shoulder_angle_yaw);
}
// Use head for counter balancing
static double hp_min = -0.1;
static double hp_max = 0.65;
// if this number is close to zero head should be hp_min
// if it is close to or above HALFPI then head should be hp_max
double arm_pitch_factor = (fabs(right_shoulder_angle_pitch) +
fabs(left_shoulder_angle_pitch)) / 2.0;
if (arm_pitch_factor >= HALFPI)
head_angle_pitch = 0.65;
else
head_angle_pitch = hp_min + ((arm_pitch_factor / HALFPI) * (hp_max - hp_min));
// head follows arm most sideways
double arm_roll_factor = (right_shoulder_angle_roll +
left_shoulder_angle_roll) / 2.0;
head_angle_yaw = arm_roll_factor * -1.1;
sensor_msgs::JointState js;
js.name.push_back("elbow_left_roll");
js.position.push_back(left_elbow_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_roll");
js.position.push_back(left_shoulder_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_pitch");
js.position.push_back(left_shoulder_angle_pitch);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_yaw");
js.position.push_back(left_shoulder_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("elbow_right_roll");
js.position.push_back(right_elbow_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_roll");
js.position.push_back(right_shoulder_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_pitch");
js.position.push_back(right_shoulder_angle_pitch);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_yaw");
js.position.push_back(right_shoulder_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("neck_yaw");
js.position.push_back(head_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("neck_pitch");
js.position.push_back(head_angle_pitch);
js.velocity.push_back(10);
joint_states_pub_.publish(js);
/////
// Following is IK method
/////
KDL::Vector left_hand_torso = left_hand; // - torso;
KDL::Vector right_hand_torso = right_hand; // - torso;
double scale = 0.0003432;
left_hand_torso = scale * left_hand_torso;
right_hand_torso = scale * right_hand_torso;
geometry_msgs::Point p;
if (joint_position_left_hand.fConfidence >= 0.5)
{
p.x = -left_hand_torso.z();
p.y = -left_hand_torso.x();
p.z = left_hand_torso.y();
left_arm_destination_pub_.publish(p);
}
if (joint_position_right_hand.fConfidence >= 0.5)
{
p.x = -right_hand_torso.z();
p.y = -right_hand_torso.x();
p.z = right_hand_torso.y();
right_arm_destination_pub_.publish(p);
}
// The robot will receive and try to mirror my position
geometry_msgs::Point torso_destination;
torso_destination.x = -torso.z() / 1000.0;
torso_destination.y = -torso.x() / 1000.0;
//robot_destination.z = torso.y() / 1000.0;
torso_destination.z = torso_pitch;
torso_destination_pub_.publish(torso_destination);
g_skel.user_id = user;
g_skel.header.stamp = ros::Time::now();
g_skel.header.frame_id = fixed_frame;
skeleton_pub_.publish(g_skel);
cmdVelPub.publish(vel);
//Visualize the trajectory of right hand in RVIZ
visualization_msgs::Marker points;
points.header.frame_id = "/openni_depth_frame";
points.header.stamp = ros::Time::now();
points.ns = "trajectory_right";
points.type = visualization_msgs::Marker::POINTS;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
points.scale.x = 0.03;
points.scale.y = 0.03;
points.color.r = 1.0;
points.color.a = 0.5;
XnPoint3D pt;
for (int k = 0; k < g_RightHandPositionHistory.Size(); ++k) {
g_RightHandPositionHistory.GetValueScreen (k, pt);
geometry_msgs::Point p;
p.x = pt.Z/1000;
p.y = pt.Y/1000;
p.z = pt.X/1000;
points.points.push_back(p);
}
marker_pub.publish(points);
//Visualize the trajectory of left hand in RVIZ
visualization_msgs::Marker point;
point.header.frame_id = "/openni_depth_frame";
point.header.stamp = ros::Time::now();
point.ns = "trajectory_left";
point.type = visualization_msgs::Marker::POINTS;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
point.scale.x = 0.03;
point.scale.y = 0.03;
point.color.b = 1.0;
point.color.a = 0.5;
XnPoint3D pt1;
for (int k = 0; k < g_LeftHandPositionHistory.Size(); ++k) {
g_LeftHandPositionHistory.GetValueScreen (k, pt1);
geometry_msgs::Point p;
p.x = pt1.Z/1000;
p.y = pt1.Y/1000;
p.z = pt1.X/1000;
point.points.push_back(p);
}
marker_pub.publish(point);
break; // only read first user
}
}
int main(int argc, char** argv) {
const double PI(3.141592653589793);
if (argc != 4) {
std::cout << "usage:" << std::endl;
std::cout << "package_scene path_scene output" << std::endl;
return 0;
}
ros::init(argc, argv, "dart_test");
ros::NodeHandle nh_;
std::string package_name( argv[1] );
std::string scene_urdf( argv[2] );
ros::Rate loop_rate(400);
ros::Publisher joint_state_pub_;
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 10);
tf::TransformBroadcaster br;
MarkerPublisher markers_pub(nh_);
std::string package_path_barrett = ros::package::getPath("barrett_hand_defs");
std::string package_path = ros::package::getPath(package_name);
// Load the Skeleton from a file
dart::utils::DartLoader loader;
loader.addPackageDirectory("barrett_hand_defs", package_path_barrett);
loader.addPackageDirectory("barrett_hand_sim_dart", package_path);
boost::shared_ptr<GraspSpecification > gspec = GraspSpecification::readFromUrdf(package_path + scene_urdf);
dart::dynamics::SkeletonPtr scene( loader.parseSkeleton(package_path + scene_urdf) );
scene->enableSelfCollision(true);
dart::dynamics::SkeletonPtr bh( loader.parseSkeleton(package_path_barrett + "/robots/barrett_hand.urdf") );
Eigen::Isometry3d tf;
tf = scene->getBodyNode("gripper_mount_link")->getRelativeTransform();
bh->getJoint(0)->setTransformFromParentBodyNode(tf);
dart::simulation::World* world = new dart::simulation::World();
world->addSkeleton(scene);
world->addSkeleton(bh);
Eigen::Vector3d grav(0,0,-1);
world->setGravity(grav);
GripperController gc;
double Kc = 400.0;
double KcDivTi = Kc / 1.0;
gc.addJoint("right_HandFingerOneKnuckleOneJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, true, false);
gc.addJoint("right_HandFingerOneKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJoint("right_HandFingerTwoKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJoint("right_HandFingerThreeKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
gc.addJointMimic("right_HandFingerTwoKnuckleOneJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, true, "right_HandFingerOneKnuckleOneJoint", 1.0, 0.0);
gc.addJointMimic("right_HandFingerOneKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerOneKnuckleTwoJoint", 0.333333, 0.0);
gc.addJointMimic("right_HandFingerTwoKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerTwoKnuckleTwoJoint", 0.333333, 0.0);
gc.addJointMimic("right_HandFingerThreeKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerThreeKnuckleTwoJoint", 0.333333, 0.0);
gc.setGoalPosition("right_HandFingerOneKnuckleOneJoint", gspec->getGoalPosition("right_HandFingerOneKnuckleOneJoint"));
gc.setGoalPosition("right_HandFingerOneKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerOneKnuckleTwoJoint"));
gc.setGoalPosition("right_HandFingerTwoKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerTwoKnuckleTwoJoint"));
gc.setGoalPosition("right_HandFingerThreeKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerThreeKnuckleTwoJoint"));
std::map<std::string, double> joint_q_map;
joint_q_map["right_HandFingerOneKnuckleOneJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleOneJoint");
joint_q_map["right_HandFingerTwoKnuckleOneJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleOneJoint");
joint_q_map["right_HandFingerOneKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleTwoJoint");
joint_q_map["right_HandFingerOneKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerOneKnuckleTwoJoint");
joint_q_map["right_HandFingerTwoKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerTwoKnuckleTwoJoint");
joint_q_map["right_HandFingerTwoKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerTwoKnuckleTwoJoint");
joint_q_map["right_HandFingerThreeKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerThreeKnuckleTwoJoint");
joint_q_map["right_HandFingerThreeKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerThreeKnuckleTwoJoint");
for (std::vector<std::string >::const_iterator it = gc.getJointNames().begin(); it != gc.getJointNames().end(); it++) {
dart::dynamics::Joint *j = bh->getJoint((*it));
j->setActuatorType(dart::dynamics::Joint::FORCE);
j->setPositionLimited(true);
j->setPosition(0, joint_q_map[(*it)]);
}
int counter = 0;
while (ros::ok()) {
world->step(false);
for (std::map<std::string, double>::iterator it = joint_q_map.begin(); it != joint_q_map.end(); it++) {
dart::dynamics::Joint *j = bh->getJoint(it->first);
it->second = j->getPosition(0);
}
gc.controlStep(joint_q_map);
// Compute the joint forces needed to compensate for Coriolis forces and
// gravity
const Eigen::VectorXd& Cg = bh->getCoriolisAndGravityForces();
for (std::map<std::string, double>::iterator it = joint_q_map.begin(); it != joint_q_map.end(); it++) {
dart::dynamics::Joint *j = bh->getJoint(it->first);
int qidx = j->getIndexInSkeleton(0);
double u = gc.getControl(it->first);
double dq = j->getVelocity(0);
if (!gc.isBackdrivable(it->first)) {
j->setPositionLowerLimit(0, std::max(j->getPositionLowerLimit(0), it->second-0.01));
}
if (gc.isStopped(it->first)) {
j->setPositionLowerLimit(0, std::max(j->getPositionLowerLimit(0), it->second-0.01));
j->setPositionUpperLimit(0, std::min(j->getPositionUpperLimit(0), it->second+0.01));
// std::cout << it->first << " " << "stopped" << std::endl;
}
j->setForce(0, 0.02*(u-dq) + Cg(qidx));
}
for (int bidx = 0; bidx < bh->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = bh->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
// std::cout << b->getName() << std::endl;
publishTransform(br, T_W_L, b->getName(), "world");
}
int m_id = 0;
for (int bidx = 0; bidx < scene->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = scene->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
publishTransform(br, T_W_L, b->getName(), "world");
for (int cidx = 0; cidx < b->getNumCollisionShapes(); cidx++) {
dart::dynamics::ConstShapePtr sh = b->getCollisionShape(cidx);
if (sh->getShapeType() == dart::dynamics::Shape::MESH) {
std::shared_ptr<const dart::dynamics::MeshShape > msh = std::static_pointer_cast<const dart::dynamics::MeshShape >(sh);
m_id = markers_pub.addMeshMarker(m_id, KDL::Vector(), 0, 1, 0, 1, 1, 1, 1, msh->getMeshUri(), b->getName());
}
}
}
markers_pub.publish();
ros::spinOnce();
loop_rate.sleep();
counter++;
if (counter < 3000) {
}
else if (counter == 3000) {
dart::dynamics::Joint::Properties prop = bh->getJoint(0)->getJointProperties();
dart::dynamics::FreeJoint::Properties prop_free;
prop_free.mName = prop_free.mName;
prop_free.mT_ParentBodyToJoint = prop.mT_ParentBodyToJoint;
prop_free.mT_ChildBodyToJoint = prop.mT_ChildBodyToJoint;
prop_free.mIsPositionLimited = false;
prop_free.mActuatorType = dart::dynamics::Joint::VELOCITY;
bh->getRootBodyNode()->changeParentJointType<dart::dynamics::FreeJoint >(prop_free);
}
else if (counter < 4000) {
bh->getDof("Joint_pos_z")->setVelocity(-0.1);
}
else {
break;
}
}
//
// generate models
//
const std::string ob_name( "graspable" );
scene->getBodyNode(ob_name)->setFrictionCoeff(0.001);
// calculate point clouds for all links and for the grasped object
std::map<std::string, pcl::PointCloud<pcl::PointNormal>::Ptr > point_clouds_map;
std::map<std::string, pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr > point_pc_clouds_map;
std::map<std::string, KDL::Frame > frames_map;
std::map<std::string, boost::shared_ptr<std::vector<KDL::Frame > > > features_map;
std::map<std::string, boost::shared_ptr<pcl::VoxelGrid<pcl::PointNormal> > > grids_map;
for (int skidx = 0; skidx < world->getNumSkeletons(); skidx++) {
dart::dynamics::SkeletonPtr sk = world->getSkeleton(skidx);
for (int bidx = 0; bidx < sk->getNumBodyNodes(); bidx++) {
dart::dynamics::BodyNode *b = sk->getBodyNode(bidx);
const Eigen::Isometry3d &tf = b->getTransform();
const std::string &body_name = b->getName();
if (body_name.find("right_Hand") != 0 && body_name != ob_name) {
continue;
}
KDL::Frame T_W_L;
EigenTfToKDL(tf, T_W_L);
std::cout << body_name << " " << b->getNumCollisionShapes() << std::endl;
for (int cidx = 0; cidx < b->getNumCollisionShapes(); cidx++) {
dart::dynamics::ConstShapePtr sh = b->getCollisionShape(cidx);
if (sh->getShapeType() == dart::dynamics::Shape::MESH) {
std::shared_ptr<const dart::dynamics::MeshShape > msh = std::static_pointer_cast<const dart::dynamics::MeshShape >(sh);
std::cout << "mesh path: " << msh->getMeshPath() << std::endl;
std::cout << "mesh uri: " << msh->getMeshUri() << std::endl;
const Eigen::Isometry3d &tf = sh->getLocalTransform();
KDL::Frame T_L_S;
EigenTfToKDL(tf, T_L_S);
KDL::Frame T_S_L = T_L_S.Inverse();
const aiScene *sc = msh->getMesh();
if (sc->mNumMeshes != 1) {
std::cout << "ERROR: sc->mNumMeshes = " << sc->mNumMeshes << std::endl;
}
int midx = 0;
// std::cout << "v: " << sc->mMeshes[midx]->mNumVertices << " f: " << sc->mMeshes[midx]->mNumFaces << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointNormal>);
uniform_sampling(sc->mMeshes[midx], 1000000, *cloud_1);
for (int pidx = 0; pidx < cloud_1->points.size(); pidx++) {
KDL::Vector pt_L = T_L_S * KDL::Vector(cloud_1->points[pidx].x, cloud_1->points[pidx].y, cloud_1->points[pidx].z);
cloud_1->points[pidx].x = pt_L.x();
cloud_1->points[pidx].y = pt_L.y();
cloud_1->points[pidx].z = pt_L.z();
}
// Voxelgrid
boost::shared_ptr<pcl::VoxelGrid<pcl::PointNormal> > grid_(new pcl::VoxelGrid<pcl::PointNormal>);
pcl::PointCloud<pcl::PointNormal>::Ptr res(new pcl::PointCloud<pcl::PointNormal>);
grid_->setDownsampleAllData(true);
grid_->setSaveLeafLayout(true);
grid_->setInputCloud(cloud_1);
grid_->setLeafSize(0.004, 0.004, 0.004);
grid_->filter (*res);
point_clouds_map[body_name] = res;
frames_map[body_name] = T_W_L;
grids_map[body_name] = grid_;
std::cout << "res->points.size(): " << res->points.size() << std::endl;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>);
// Setup the principal curvatures computation
pcl::PrincipalCurvaturesEstimation<pcl::PointNormal, pcl::PointNormal, pcl::PrincipalCurvatures> principalCurvaturesEstimation;
// Provide the original point cloud (without normals)
principalCurvaturesEstimation.setInputCloud (res);
// Provide the point cloud with normals
principalCurvaturesEstimation.setInputNormals(res);
// Use the same KdTree from the normal estimation
principalCurvaturesEstimation.setSearchMethod (tree);
principalCurvaturesEstimation.setRadiusSearch(0.02);
// Actually compute the principal curvatures
pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principalCurvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
principalCurvaturesEstimation.compute (*principalCurvatures);
point_pc_clouds_map[body_name] = principalCurvatures;
features_map[body_name].reset( new std::vector<KDL::Frame >(res->points.size()) );
for (int pidx = 0; pidx < res->points.size(); pidx++) {
KDL::Vector nx, ny, nz(res->points[pidx].normal[0], res->points[pidx].normal[1], res->points[pidx].normal[2]);
if ( std::fabs( principalCurvatures->points[pidx].pc1 - principalCurvatures->points[pidx].pc2 ) > 0.001) {
nx = KDL::Vector(principalCurvatures->points[pidx].principal_curvature[0], principalCurvatures->points[pidx].principal_curvature[1], principalCurvatures->points[pidx].principal_curvature[2]);
}
else {
if (std::fabs(nz.z()) < 0.7) {
nx = KDL::Vector(0, 0, 1);
}
else {
nx = KDL::Vector(1, 0, 0);
}
}
ny = nz * nx;
nx = ny * nz;
nx.Normalize();
ny.Normalize();
nz.Normalize();
(*features_map[body_name])[pidx] = KDL::Frame( KDL::Rotation(nx, ny, nz), KDL::Vector(res->points[pidx].x, res->points[pidx].y, res->points[pidx].z) );
}
}
}
}
}
const double sigma_p = 0.01;//05;
const double sigma_q = 10.0/180.0*PI;//100.0;
const double sigma_r = 0.2;//05;
double sigma_c = 5.0/180.0*PI;
int m_id = 101;
// generate object model
boost::shared_ptr<ObjectModel > om(new ObjectModel);
for (int pidx = 0; pidx < point_clouds_map[ob_name]->points.size(); pidx++) {
if (point_pc_clouds_map[ob_name]->points[pidx].pc1 > 1.1 * point_pc_clouds_map[ob_name]->points[pidx].pc2) {
// e.g. pc1=1, pc2=0
// edge
om->addPointFeature((*features_map[ob_name])[pidx] * KDL::Frame(KDL::Rotation::RotZ(PI)), point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
om->addPointFeature((*features_map[ob_name])[pidx], point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
}
else {
for (double angle = 0.0; angle < 359.0/180.0*PI; angle += 20.0/180.0*PI) {
om->addPointFeature((*features_map[ob_name])[pidx] * KDL::Frame(KDL::Rotation::RotZ(angle)), point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
}
}
}
std::cout << "om.getPointFeatures().size(): " << om->getPointFeatures().size() << std::endl;
KDL::Frame T_W_O = frames_map[ob_name];
// generate collision model
std::map<std::string, std::list<std::pair<int, double> > > link_pt_map;
boost::shared_ptr<CollisionModel > cm(new CollisionModel);
cm->setSamplerParameters(sigma_p, sigma_q, sigma_r);
std::list<std::string > gripper_link_names;
for (int bidx = 0; bidx < bh->getNumBodyNodes(); bidx++) {
const std::string &link_name = bh->getBodyNode(bidx)->getName();
gripper_link_names.push_back(link_name);
}
double dist_range = 0.01;
for (std::list<std::string >::const_iterator nit = gripper_link_names.begin(); nit != gripper_link_names.end(); nit++) {
const std::string &link_name = (*nit);
if (point_clouds_map.find( link_name ) == point_clouds_map.end()) {
continue;
}
cm->addLinkContacts(dist_range, link_name, point_clouds_map[link_name], frames_map[link_name],
om->getPointFeatures(), T_W_O);
}
// generate hand configuration model
boost::shared_ptr<HandConfigurationModel > hm(new HandConfigurationModel);
std::map<std::string, double> joint_q_map_before( joint_q_map );
double angleDiffKnuckleTwo = 15.0/180.0*PI;
joint_q_map_before["right_HandFingerOneKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerTwoKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerThreeKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
joint_q_map_before["right_HandFingerOneKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
joint_q_map_before["right_HandFingerTwoKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
joint_q_map_before["right_HandFingerThreeKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
hm->generateModel(joint_q_map_before, joint_q_map, 1.0, 10, sigma_c);
writeToXml(argv[3], cm, hm);
return 0;
}
void processKinect(KinectController &kinect_controller)
{
XnUserID users[15];
XnUInt16 users_count = 15;
xn::UserGenerator& UserGenerator = kinect_controller.getUserGenerator();
UserGenerator.GetUsers(users, users_count);
pi_tracker::Skeleton g_skel;
for (int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
if (!UserGenerator.GetSkeletonCap().IsTracking(user))
continue;
publishTransform(kinect_controller, user, XN_SKEL_HEAD, fixed_frame, "head", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_NECK, fixed_frame, "neck", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_TORSO, fixed_frame, "torso", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_SHOULDER, fixed_frame, "left_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_ELBOW, fixed_frame, "left_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HAND, fixed_frame, "left_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_SHOULDER, fixed_frame, "right_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_ELBOW, fixed_frame, "right_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HAND, fixed_frame, "right_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HIP, fixed_frame, "left_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_KNEE, fixed_frame, "left_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_FOOT, fixed_frame, "left_foot", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HIP, fixed_frame, "right_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_KNEE, fixed_frame, "right_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_FOOT, fixed_frame, "right_foot", g_skel);
g_skel.user_id = user;
g_skel.header.stamp = ros::Time::now();
g_skel.header.frame_id = fixed_frame;
skeleton_pub_.publish(g_skel);
break; // only read first user
}
}
void spin() {
// initialize random seed
srand(time(NULL));
// dynamics model
boost::shared_ptr<DynamicModel > dyn_model( new DynModelPlanar5() );
std::string robot_description_str;
std::string robot_semantic_description_str;
nh_.getParam("/robot_description", robot_description_str);
nh_.getParam("/robot_semantic_description", robot_semantic_description_str);
//
// collision model
//
boost::shared_ptr<self_collision::CollisionModel> col_model = self_collision::CollisionModel::parseURDF(robot_description_str);
col_model->parseSRDF(robot_semantic_description_str);
col_model->generateCollisionPairs();
// external collision objects - part of virtual link connected to the base link
self_collision::Link::VecPtrCollision col_array;
col_array.push_back( self_collision::createCollisionCapsule(0.2, 0.3, KDL::Frame(KDL::Rotation::RotX(90.0/180.0*PI), KDL::Vector(1, 0.5, 0))) );
if (!col_model->addLink("env_link", "base", col_array)) {
ROS_ERROR("ERROR: could not add external collision objects to the collision model");
return;
}
col_model->generateCollisionPairs();
//
// robot state
//
std::vector<std::string > joint_names;
joint_names.push_back("0_joint");
joint_names.push_back("1_joint");
joint_names.push_back("2_joint");
joint_names.push_back("3_joint");
joint_names.push_back("4_joint");
int ndof = joint_names.size();
Eigen::VectorXd q, dq, ddq, torque;
q.resize( ndof );
dq.resize( ndof );
ddq.resize( ndof );
torque.resize( ndof );
for (int q_idx = 0; q_idx < ndof; q_idx++) {
q[q_idx] = -0.1;
dq[q_idx] = 0.0;
ddq[q_idx] = 0.0;
torque[q_idx] = 0.0;
}
std::string effector_name = "effector";
int effector_idx = col_model->getLinkIndex(effector_name);
//
// kinematic model
//
boost::shared_ptr<KinematicModel> kin_model( new KinematicModel(robot_description_str, joint_names) );
KinematicModel::Jacobian J_r_HAND_6, J_r_HAND;
J_r_HAND_6.resize(6, ndof);
J_r_HAND.resize(3, ndof);
std::vector<KDL::Frame > links_fk(col_model->getLinksCount());
// joint limits
Eigen::VectorXd lower_limit(ndof), upper_limit(ndof), limit_range(ndof), max_trq(ndof);
int q_idx = 0;
for (std::vector<std::string >::const_iterator name_it = joint_names.begin(); name_it != joint_names.end(); name_it++, q_idx++) {
lower_limit[q_idx] = kin_model->getLowerLimit(q_idx);
upper_limit[q_idx] = kin_model->getUpperLimit(q_idx);
limit_range[q_idx] = 10.0/180.0*PI;
max_trq[q_idx] = 10.0;
}
Eigen::VectorXd max_q(ndof);
max_q(0) = 10.0/180.0*PI;
max_q(1) = 20.0/180.0*PI;
max_q(2) = 20.0/180.0*PI;
max_q(3) = 30.0/180.0*PI;
max_q(4) = 40.0/180.0*PI;
boost::shared_ptr<DynamicsSimulatorHandPose> sim( new DynamicsSimulatorHandPose(ndof, 6, effector_name, col_model, kin_model, dyn_model, joint_names, max_q) );
/*
//
// Tasks declaration
//
Task_JLC task_JLC(lower_limit, upper_limit, limit_range, max_trq);
Task_WCC task_WCC(ndof, 3, 4);
double activation_dist = 0.05;
Task_COL task_COL(ndof, activation_dist, 10.0, kin_model, col_model);
Task_HAND task_HAND(ndof, 3);
*/
/*
while (ros::ok()) {
//
// wrist collision constraint
//
Eigen::VectorXd torque_WCC(ndof);
Eigen::MatrixXd N_WCC(ndof, ndof);
task_WCC.compute(q, dq, dyn_model->getM(), dyn_model->getInvM(), torque_WCC, N_WCC, markers_pub_);
markers_pub_.publish();
ros::spinOnce();
char ch = getchar();
if (ch == 'a') {
q(3) -= 0.1;
}
else if (ch == 'd') {
q(3) += 0.1;
}
else if (ch == 's') {
q(4) -= 0.1;
}
else if (ch == 'w') {
q(4) += 0.1;
}
}
return;
*/
// loop variables
ros::Time last_time = ros::Time::now();
KDL::Frame r_HAND_target;
int loop_counter = 10000;
ros::Rate loop_rate(100);
while (true) {
generatePossiblePose(r_HAND_target, q, ndof, effector_name, col_model, kin_model);
sim->setState(q, dq, ddq);
sim->setTarget(r_HAND_target);
sim->oneStep();
if (!sim->inCollision()) {
break;
}
}
while (ros::ok()) {
if (loop_counter > 500) {
Eigen::VectorXd q_tmp(ndof);
generatePossiblePose(r_HAND_target, q_tmp, ndof, effector_name, col_model, kin_model);
sim->setTarget(r_HAND_target);
publishTransform(br, r_HAND_target, "effector_dest", "base");
loop_counter = 0;
}
loop_counter += 1;
sim->oneStep(&markers_pub_, 3000);
/* if (sim->inCollision() && !collision_in_prev_step) {
collision_in_prev_step = true;
std::cout << "collision begin" << std::endl;
}
else if (!sim->inCollision() && collision_in_prev_step) {
collision_in_prev_step = false;
std::cout << "collision end" << std::endl;
}
*/
sim->getState(q, dq, ddq);
// publish markers and robot state with limited rate
ros::Duration time_elapsed = ros::Time::now() - last_time;
if (time_elapsed.toSec() > 0.05) {
// calculate forward kinematics for all links
for (int l_idx = 0; l_idx < col_model->getLinksCount(); l_idx++) {
kin_model->calculateFk(links_fk[l_idx], col_model->getLinkName(l_idx), q);
}
publishJointState(joint_state_pub_, q, joint_names);
int m_id = 0;
m_id = addRobotModelVis(markers_pub_, m_id, col_model, links_fk);
markers_pub_.publish();
ros::Time last_time = ros::Time::now();
}
ros::spinOnce();
loop_rate.sleep();
}
}