void MainWindow::addButtons()
{
URDFparser* parser = URDFparser::getInstance();
RobotModel rm = parser->rm;
map<QString, Link> linksMap = rm.getLinks();
map<QString, Joint> jointsMap = rm.getJoints();
vector<Joint> joints = rm.sortJoints(jointsMap);
cb = new QComboBox(ui->frame);
QStringList* sl = new QStringList();
button = new QPushButton("Test", ui->frame);
button->show();
for(uint i=0; i<joints.size();i++)
{
if(linksMap[joints.at(i).getParent().getLink()].getVisual().size()>0)
{
if(!sl->contains(joints.at(i).getParent().getLink()))
sl->append(joints.at(i).getParent().getLink());
}
if(linksMap[joints.at(i).getChild().getLink()].getVisual().size()>0)
{
if(!sl->contains(joints.at(i).getChild().getLink()))
sl->append(joints.at(i).getChild().getLink());
}
}
cb->addItems(*sl);
cb->setFixedWidth(150);
button->setFixedWidth(40);
connect(button, SIGNAL(released()), this, SLOT(testButtonClicked()));
ui->formLayout->addRow(cb, button);
}
//Slot Unwrapper for internet datagram (vision)
void SimulatorCore::UnWrapVisionPacket(SSL_WrapperPacket iPacketTeam){
SSL_DetectionRobot lPacketRobotBlue;
SSL_DetectionRobot lPacketRobotYellow;
SSL_DetectionBall lPacketBall = iPacketTeam.detection().balls(0);
int lBlueSize = iPacketTeam.detection().robots_blue_size();
int lYellowSize = iPacketTeam.detection().robots_blue_size();
mBall->setPosition(lPacketBall.x(),lPacketBall.y());
for(int i = 0; i < lBlueSize; ++i){
lPacketRobotBlue = iPacketTeam.detection().robots_blue(i);
int RobotId = lPacketRobotBlue.robot_id();
if(RobotId > mNbPlayerPerTeam - 1){
break; //TODO clean this with dynamic player allocation
}
RobotModel* lRobotBlue = mBlueTeam->GetRobot(RobotId);
lRobotBlue->setPose(Pose(lPacketRobotBlue.x(),lPacketRobotBlue.y(),
lPacketRobotBlue.orientation()));
// TODO should be in another function
lRobotBlue->UpdateSimulationData();
}
for(int i = 0; i < lYellowSize; ++i){
lPacketRobotYellow = iPacketTeam.detection().robots_yellow(i);
int RobotId = lPacketRobotYellow.robot_id();
if(RobotId > mNbPlayerPerTeam - 1){
break; //TODO clean this with dynamic player allocation
}
RobotModel* lRobotYellow = mYellowTeam->GetRobot(RobotId);
lRobotYellow->setPose(Pose(lPacketRobotYellow.x(),lPacketRobotYellow.y(),
lPacketRobotYellow.orientation()));
}
this->sendCommandToGrSim(); //TODO not clear, send back the new command to grsim
}
void SimulatorCore::sendCommandToGrSim(){
for(int i = 0; i < mNbPlayerPerTeam; ++i){
RobotModel* lRobotBlue = mBlueTeam->GetRobot(i);
mCommandOutput->AddgrSimCommand(lRobotBlue->getCommand(),
BLUE,
lRobotBlue->getId());
}
mCommandOutput->SendCommandDatagram();
}
void SimulatorCore::UnWrapCommandPacket(grSim_Packet iPacket){
int size = iPacket.commands().robot_commands().size();
Pose lCommand;
for(int i = 0; i < size; ++i){
int lID = iPacket.commands().robot_commands().Get(i).id();
RobotModel* lRobot = mBlueTeam->GetRobot(lID);
lCommand = unWrapCommand(&iPacket, lID);
lRobot->receiveCommand(lCommand);
}
}
void RobotInit() {
RefreshAllIni();
robot->ResetTimer();
robot->Reset();
auton->ListOptions();
visionController->Disable();
std::thread cameraThread(CameraThread);
cameraThread.detach();
}
void SpinReader::loadTFMFromProgramOptions(const bpo::variables_map & vm)
{
RobotModel model;
if( vm.count("robot_description") )
model.addFile(vm["robot_description"].as<std::string>());
else if( ! ros::param::has("/driving/robot_description") )
BOOST_THROW_EXCEPTION(stdr::ex::ExceptionBase() <<stdr::ex::MsgInfo(
"You must provide a model description, either on the command line, or as a rosparam."));
model.addParam("/driving/robot_description");
BOOST_FOREACH(const tf::StampedTransform& t, model.getStaticTransforms()) {
tf_listener_.addStaticTransform(t);
}
}
void AutonomousInit() {
auton->Stop();
RefreshAllIni();
robot->ResetTimer();
robot->ResetEncoders();
driveController->Reset();
//Resets timer variables
currTimeSec = 0.0;
lastTimeSec = 0.0;
deltaTimeSec = 0.0;
visionController->Enable();
auton->Start();
}
void TeleopInit() {
lights->SetEnabledRoutine();
auton->Stop();
RefreshAllIni();
robot->ResetTimer();
robot->ResetEncoders();
driveController->Reset();
climberController->Reset();
gearController->Reset();
//Resets timer variables
currTimeSec = 0.0;
lastTimeSec = 0.0;
deltaTimeSec = 0.0;
visionController->Enable();
}
void Model::addRobotModel(robotModel::TwoDRobotModel &robotModel, const QPointF &pos)
{
RobotModel *robot = new RobotModel(robotModel, mSettings, this);
robot->setPosition(pos);
connect(&mTimeline, &Timeline::started, robot, &RobotModel::reinit);
connect(&mTimeline, &Timeline::stopped, robot, &RobotModel::stopRobot);
connect(&mTimeline, &Timeline::tick, robot, &RobotModel::recalculateParams);
connect(&mTimeline, &Timeline::nextFrame, robot, &RobotModel::nextFragment);
auto resetPhysics = [this, robot]() { robot->resetPhysics(mWorldModel, mTimeline); };
connect(&mSettings, &Settings::physicsChanged, resetPhysics);
resetPhysics();
mRobotModels.append(robot);
emit robotAdded(robot);
}
void DisabledInit() {
auton->Stop();
RefreshAllIni();
robot->ResetEncoders();
driveController->Reset();
climberController->Reset();
visionController->Disable();
lights->SetDisabledRoutine();
}
void RobotModelProvider::update(RobotModel& robotModel)
{
robotModel.setJointData(theFilteredJointData, theRobotDimensions, theMassCalibration);
DECLARE_DEBUG_DRAWING3D("module:RobotModelProvider:massOffsets", "robot",
{
for(int i = 0; i < MassCalibration::numOfLimbs; ++i)
{
const Vector3<>& v(robotModel.limbs[i] * theMassCalibration.masses[i].offset);
SPHERE3D("module:RobotModelProvider:massOffsets", v.x, v.y, v.z, 3, ColorRGBA(0, 200, 0));
}
});
void SimulatorCore::setRobotsConfig(GaussianModel iGaussX, GaussianModel iGaussY,
GaussianModel iGaussTheta){
for(int i = 0; i < mNbPlayerPerTeam; ++i){
RobotModel *lBlue = mBlueTeam->GetRobot(i);
lBlue->setGaussX(iGaussX);
lBlue->setGaussY(iGaussY);
lBlue->setGaussTheta(iGaussTheta);
RobotModel *lYellow = mYellowTeam->GetRobot(i);
lYellow->setGaussX(iGaussX);
lYellow->setGaussY(iGaussY);
lYellow->setGaussTheta(iGaussTheta);
}
}
void smartTask()
{
RobotServer rs(&physicalRobot);
cout << "-----------server initialized-----------\n";
physicalRobot.setSrv(&rs);
physicalRobot.placeRobotInMap(&world);
robotInterface->startSensorPoller();
#if 0
sleep(1);
physicalRobot.move(1050);
#endif
#if 0
sleep(1);
physicalRobot.rotate(DEG_TO_RAD(180));
#endif
while (1)
;
}
SpeedObjective::
SpeedObjective(const DynamicWindow & dynamic_window,
const RobotModel & robot_model)
: Objective(dynamic_window),
sdMax(robot_model.SdMax()),
m_robot_model(robot_model),
m_forward(dimension, dimension),
m_backward(dimension, dimension),
m_slow(dimension, dimension),
m_strict_forward(dimension, dimension),
m_strict_backward(dimension, dimension),
m_strict_slow(dimension, dimension),
m_current( & m_forward),
m_goForward(true)
{
}
void TeleopPeriodic() {
dashboardLogger->UpdateEssentialData();
//Updates timer
lastTimeSec = currTimeSec;
currTimeSec = robot->GetTime();
deltaTimeSec = currTimeSec - lastTimeSec;
//Reads controls and updates controllers accordingly
//RefreshAllIni();
humanControl->ReadControls();
driveController->Update(currTimeSec, deltaTimeSec);
climberController->Update();
//visionController->Update();
gearController->Update();
if (humanControl->GetJoystickValue(RemoteControl::kOperatorJoy, RemoteControl::kRY) > 0.2) {
lights->Climbing();
} else if (humanControl->GetShoutRoutineDesired()) {
lights->SetShoutRoutine();
} else if (humanControl->GetGearTitlerIntakeDesired()) {
lights->GearIntake();
} else if (humanControl->GetGearTitlerOuttakeDesired()) {
lights->GearOuttake();
} else if (humanControl->GetSlowDriveTier1Desired()
&& humanControl->GetSlowDriveTier2Desired()) {
lights->Brake2();
} else if (humanControl->GetSlowDriveTier1Desired()) {
lights->Brake1();
} else {
lights->SetEnabledRoutine();
}
}
BubbleBand::
BubbleBand(const RobotModel & robot_model,
const Odometry & odometry,
const Multiscanner & multiscanner,
BubbleList::Parameters _parameters)
: parameters(_parameters),
robot_radius(robot_model.GetHull()->CalculateRadius()),
robot_diameter(2 * robot_radius),
ignore_radius(0.9 * robot_radius),
deletion_diameter(1.8 * robot_diameter),
addition_diameter(1.2 * robot_diameter),
m_odometry(odometry),
m_multiscanner(multiscanner),
m_bubble_factory(new BubbleFactory(50)), // hax: hardcoded initlevel
m_replan_handler(new ReplanHandler(*this, *m_bubble_factory)),
m_frame(new Frame()),
m_active_blist(new BubbleList(*this, *m_bubble_factory, _parameters)),
m_reaction_radius(2.0 * robot_radius),
m_replan_request(false),
m_state(NOBAND),
m_planstep(IDLE)
{
}
void Model::deserialize(const QDomDocument &xml)
{
const QDomNodeList worldList = xml.elementsByTagName("world");
const QDomNodeList robotsList = xml.elementsByTagName("robots");
const QDomElement constraints = xml.documentElement().firstChildElement("constraints");
if (mChecker) {
/// @todo: should we handle if it returned false?
mChecker->parseConstraints(constraints);
}
if (worldList.count() != 1) {
return;
}
mWorldModel.deserialize(worldList.at(0).toElement());
if (robotsList.count() != 1) {
// need for backward compatibility with old format
const QDomNodeList robotList = xml.elementsByTagName("robot");
if (robotList.count() != 1) {
/// @todo Report error
return;
}
mRobotModels.at(0)->deserialize(robotList.at(0).toElement());
mRobotModels.at(0)->configuration().deserialize(robotList.at(0).toElement());
return;
}
QMutableListIterator<RobotModel *> iterator(mRobotModels);
const bool oneRobot = robotsList.at(0).toElement().elementsByTagName("robot").size() == 1
&& mRobotModels.size() == 1;
while(iterator.hasNext()) {
bool exist = false;
RobotModel *robotModel = iterator.next();
for (QDomElement element = robotsList.at(0).firstChildElement("robot"); !element.isNull();
element = element.nextSiblingElement("robot")) {
if (robotModel->info().robotId() == element.toElement().attribute("id")) {
robotModel->deserialize(element);
robotModel->configuration().deserialize(element);
exist = true;
robotsList.at(0).removeChild(static_cast<QDomNode>(element));
break;
}
}
if (!exist && !oneRobot) {
iterator.remove();
emit robotRemoved(robotModel);
delete robotModel;
}
}
if (oneRobot && !robotsList.at(0).firstChildElement("robot").isNull()) {
QDomElement element = robotsList.at(0).firstChildElement("robot");
mRobotModels.at(0)->deserialize(element);
} else {
for (QDomElement element = robotsList.at(0).firstChildElement("robot"); !element.isNull();
element = element.nextSiblingElement("robot")) {
twoDModel::robotModel::NullTwoDRobotModel *robotModel = new twoDModel::robotModel::NullTwoDRobotModel(
element.attribute("id"));
addRobotModel(*robotModel);
mRobotModels.last()->deserialize(element);
}
}
}
#include "RobotModel.hpp"
const RobotModel RobotModel2015 = []() {
RobotModel model;
model.WheelRadius = 0.02856;
// note: wheels are numbered clockwise, starting with the top-right
// TODO(ashaw596): Check angles.
model.WheelAngles = {
DegreesToRadians(38), DegreesToRadians(315), DegreesToRadians(225),
DegreesToRadians(142),
};
model.WheelDist = 0.0798576;
model.DutyCycleMultiplier = 9; // TODO: tune this value
model.recalculateBotToWheel();
return model;
}();
int main(int argc, char **argv)
{
//kinect::kinectbody hola;
//############################################
//INFORMACION DEL BAXTER
//*****************************************
std::string baxter_description = ros::package::getPath("baxter_description");
std::string model_name = baxter_description + "/urdf/baxter.urdf";
RobotModel* robot = new RobotModel();
bool has_floating_base = false;
if (!robot->loadURDF(model_name, has_floating_base)){
return -1;}
else{
std::cout << "Robot " << model_name << " loaded." << std::endl;}
unsigned int ndof_full = robot->ndof();
std::cout << "Reading initial sensor values ..." << std::endl;
// Get the joint names and joint limits
std::vector<std::string> jnames;
std::vector<double> qmin, qmax, dqmax;
jnames = robot->jointNames();
qmin = robot->jointMinAngularLimits();
qmax = robot->jointMaxAngularLimits();
dqmax = robot->jointVelocityLimits();
//############################################
//INICIO DEL PROCESO DE RECEPCION
//*****************************************
ros::init(argc, argv, "n_sendbaxter");
ros::NodeHandle nh;
for(int i=0;i<jnames.size();i++){
cout<<jnames[i]<<endl;
}
KinectPoints kpoints;
//Suscriber
//ros::Subscriber sub_1 = nh.subscribe("kinect_data", 1000, &KinectPoints::readKinectPoints, &kpoints);
//JointSensors jsensor;
//ros::Subscriber sub_2 = nh.subscribe("robot/joint_states", 1000,&JointSensors::readJointSensors, &jsensor);
//"kinect_points"
std::cout << "Reading initial sensor values ..." << std::endl;
ros::Rate iter_rate(1000); // Hz
unsigned int niter=0, max_iter = 1e3;
//unsigned int ndof_sensed = 222;
//unsigned int ndof_sensed = jsensor.sensedValue()->position.size();
//ndof_sensed = jsensor.sensedValue()->name.size();
ros::spinOnce();
iter_rate.sleep();
//std::cout << "Found " << ndof_sensed << " sensed joints" << std::endl;
Eigen::VectorXd qsensed = Eigen::VectorXd::Zero(ndof_full);
//jsensor.getSensedJointsRBDL(qsensed, ndof_sensed);
Eigen::VectorXd q=Eigen::MatrixXd::Constant(16, 1, 0.5);
cout<<q<<endl;
Eigen::Vector3d w(0,0,0);
Eigen::Vector3d v(0,0,0);
Eigen::MatrixXd qposition(3,20);
int link_number={1};
//Eigen::VectorXd qposition=Eigen::MatrixXd::Constant(15, 1, 0.0);
cout <<"full: "<<ndof_full<<endl;
cout<<"LINK position"<<endl;
std::vector< std::vector<double> > P;
std::vector< std::vector<double> > Q_orientation;
P.resize(6);
Q_orientation.resize(6);
//############################################
//INICIO DEL PROCESO DE ENVIO
//*****************************************
ros::Publisher pub = nh.advertise<sensor_msgs::JointState>("joint_states", 1000);
sensor_msgs::JointState jcmd;
jcmd.name.resize(19);
jcmd.position.resize(19);
jcmd.velocity.resize(19);
jcmd.effort.resize(19);
jcmd.name[0]="head_pan";
//jcmd.name[1]="l_gripper_l_finger_joint";
//jcmd.name[2]="l_gripper_r_finger_joint";
jcmd.name[1]="left_s0";
jcmd.name[2]="left_s1";
jcmd.name[3]="left_e0";
jcmd.name[4]="left_e1";
jcmd.name[5]="left_w0";
jcmd.name[6]="left_w1";
jcmd.name[7]="left_w2";
//jcmd.name[10]="r_gripper_l_finger_joint";
//jcmd.name[11]="r_gripper_r_finger_joint";
jcmd.name[8]="right_s0";
jcmd.name[9]="right_s1";
jcmd.name[10]="right_e0";
jcmd.name[11]="right_e1";
jcmd.name[12]="right_w0";
jcmd.name[13]="right_w1";
jcmd.name[14]="right_w2";
jcmd.name[15]="l_gripper_l_finger_joint";
jcmd.name[16]="l_gripper_r_finger_joint";
jcmd.name[17]="r_gripper_l_finger_joint";
jcmd.name[18]="r_gripper_r_finger_joint";
for(int j=0; j<19;j++){
jcmd.position[j]=0.0;
//jcmd.velocity[j]=0.2;
//jcmd.effort[j]=0.2;
}
pub.publish(jcmd);
// ************************************
// Tasks and Inverse Kinematics Solver
// ************************************
// Sampling time
unsigned int f = 200; // Frequency
double dt = static_cast<double>(1.0/f);
KineSolverWQP solver(robot, qsensed, dt);
solver.setJointLimits(qmin, qmax, dqmax);
//KineTask* taskrh = new KineTaskPose(robot, RGRIPPER, "position");
KineTask* taskrh = new KineTaskPose(robot, 14, "position");
taskrh->setGain(1.0);
KineTask* tasklh = new KineTaskPose(robot, 7, "position");
tasklh->setGain(1.0);
KineTask* taskre = new KineTaskPose(robot, 11, "position");
taskre->setGain(50.0);
KineTask* taskle = new KineTaskPose(robot, 4, "position");
taskle->setGain(50.0);
Eigen::VectorXd P_right_wrist;
//Eigen::VectorXd P_right_elbow;
Eigen::VectorXd P_left_wrist;
//Eigen::VectorXd P_left_elbow;
solver.pushTask(taskrh);
solver.pushTask(tasklh);
// solver.pushTask(taskre);
//solver.pushTask(taskle);
Eigen::VectorXd qdes;
ros::Rate rate(f); // Hz
//#######################################################
//############################################
//ROS
//*****************************************
//############################################
//PROCESO MARKER
//*****************************************
/*
double red[3] = {1.,0.,0.};
double green[3] = {0.,1.,0.};
double blue[3] = {0.,0.,1.};
double yellow[3] = {0.5,0.5,0.};
BallMarker* marker_current_head;
BallMarker* marker_current_base;
BallMarker* marker_current_right_0;
BallMarker* marker_current_right_1;
BallMarker* marker_current_right_2;
BallMarker* marker_current_right_3;
BallMarker* marker_current_right_4;
BallMarker* marker_current_right_5;
BallMarker* marker_current_right_6;
BallMarker* marker_current_left_0;
BallMarker* marker_current_left_1;
BallMarker* marker_current_left_2;
BallMarker* marker_current_left_3;
BallMarker* marker_current_left_4;
BallMarker* marker_current_left_5;
BallMarker* marker_current_left_6;
//marker_current_base = new BallMarker(nq, red);
//marker_current_head = new BallMarker(nq, blue);
marker_current_right_0 = new BallMarker(nh, green);
marker_current_right_1 = new BallMarker(nh, red);
marker_current_right_2 = new BallMarker(nh, green);
//marker_current_right_3 = new BallMarker(nq, green);
//marker_current_right_4 = new BallMarker(nq, green);
//marker_current_right_5 = new BallMarker(nq, green);
//marker_current_right_6 = new BallMarker(nq, green);
marker_current_left_0 = new BallMarker(nh, yellow);
marker_current_left_1 = new BallMarker(nh, yellow);
//marker_current_left_2 = new BallMarker(nq, yellow);
//marker_current_left_3 = new BallMarker(nq, yellow);
//marker_current_left_4 = new BallMarker(nq, yellow);
//marker_current_left_5 = new BallMarker(nq, yellow);
//marker_current_left_6 = new BallMarker(nq, yellow);
*/
Eigen::VectorXd Prwrist;
taskrh->getSensedValue(qsensed, Prwrist);
cout<< Prwrist[0] <<endl;
cout<< Prwrist[1] <<endl ;
cout << Prwrist[2] <<endl ;
while(ros::ok())
{
//std::cout<<"size: "<< kpoints.getPoints()->pose.size() << std::endl;
jcmd.header.stamp = ros::Time::now();
///////////////////////////////////////////////////////////////
//Datos del baxter
double L1 = 0.45;//Del hombro al codo
double L2 = 0.50;//Del codo a la mano
//L2=111
// if (kpoints.getPoints()->pose.size() > 0){
// cout<<"ok_body.size"<<endl;
//Programacion que recibe el brazo derecho
// for (int k=0;k<3;k++){
// P[k].resize(3);
//A partir de eso P[0][k]=(0,0,0)
// P[k][0] = (-kpoints.getPoints()->pose[k].position.z)-(-kpoints.getPoints()->pose[0].position.z);
// P[k][1] = (-kpoints.getPoints()->pose[k].position.x)-(-kpoints.getPoints()->pose[0].position.x);
// P[k][2] = (kpoints.getPoints()->pose[k].position.y)-(kpoints.getPoints()->pose[0].position.y);
// }
//Construimos los puntos (0,0,0); P1; P2
//Hallamos el modulo M1 de P1
//cout<<"ok_4"<<endl;
// double M1= sqrt(pow(P[1][0],2.0)+pow(P[1][1],2.0)+pow(P[1][2],2.0));
//Hallamos el modulo M2 de (P2-P1)
// double M2= sqrt(pow(P[2][0]- P[1][0], 2.0) + pow(P[2][1]- P[1][1], 2.0) + pow(P[2][2]- P[1][2], 2.0));
//Determimos la proporcionalidades
// double Q1 = L1 / M1;
// double Q2 = L2 / M2;
//std::cout<< "M2: "<<M2<<std::endl;
//Redfinimos P1
//cout<<"ok_5"<<endl;
// P[0][0] = 0.14;
// P[0][1] = -0.2;
// P[0][2] = 0.40;
//P[0][1] = 0.098;
//[0][2] = 0.100
//Redefinimos P2
// P[2][0] = Q2*(P[2][0] - P[1][0]);
// P[2][1] = Q2*(P[2][1] - P[1][1]);
// P[2][2] = Q2*(P[2][2] - P[1][2]);
//Redfinimos P1
// P[1][0] = P[0][0]+Q1*P[1][0];
// P[1][1] = P[0][1]+Q1*P[1][1];
// P[1][2] = P[0][2]+Q1*P[1][2];
//double M4= sqrt(pow(P[1][0],2)+pow(P[1][1],2)+pow(P[1][2],2));
//std::cout<< "M4: "<<M4<<std::endl;
// P[2][0] = P[2][0]+P[1][0];
// P[2][1] = P[2][1]+P[1][1];
// P[2][2] = P[2][2]+P[1][2];
//cout<<P[2][2]<<endl;
//Programacion que recibe el brazo izquierdo
// for (unsigned k=(P.size()/2);k<P.size();k++){
// P[k].resize(3);
// P[k][0] = (-kpoints.getPoints()->pose[k].position.z)-(-kpoints.getPoints()->pose[3].position.z);
// P[k][1] = (-kpoints.getPoints()->pose[k].position.x)-(-kpoints.getPoints()->pose[3].position.x);
// P[k][2] = (kpoints.getPoints()->pose[k].position.y)-(kpoints.getPoints()->pose[3].position.y);
//std::cout << "k 2: " << k << std::endl;
// }
//Hallamos el modulo M1 de P4
// double M3 = sqrt(pow(P[4][0], 2.0) + pow(P[4][1], 2.0) + pow(P[4][2], 2.0));
//Hallamos el modulo M2 de (P2-P1)
// double M4 = sqrt(pow(P[5][0] - P[4][0], 2.0) + pow(P[5][1] - P[4][1], 2.0) + pow(P[5][2] - P[4][2], 2.0));
// double Q3 = L1 / M3;
// double Q4 = L2 / M4;
//Construimos los puntos (0,0,0); P4; P5
// P[3][0] = 0.14;
// P[3][1] = -0.27;
// P[3][2] = 0.40;
//P[0][1] = 0.098;
//[0][2] = 0.100
//Redefinimos P5
// P[5][0] = Q4*(P[5][0] - P[4][0]);
// P[5][1] = Q4*(P[5][1] - P[4][1]);
// P[5][2] = Q4*(P[5][2] - P[4][2]);
//Redfinimos P4
// P[4][0] = P[3][0]+Q3*P[4][0];
// P[4][1] = P[3][1]+Q3*P[4][1];
// P[4][2] = P[3][2]+Q3*P[4][2];
//double M4= sqrt(pow(P[1][0],2)+pow(P[1][1],2)+pow(P[1][2],2));
//std::cout<< "M4: "<<M4<<std::endl;
// P[5][0] = P[5][0]+P[4][0];
// P[5][1] = P[5][1]+P[4][1];
// P[5][2] = P[5][2]+P[4][2];
//#######################################################
// for (int k=0;k<6;k++){
// Q_orientation[k].resize(3);
// Q_orientation[k] = conv((-kpoints.getPoints()->pose[k].orientation.x),(-kpoints.getPoints()->pose[k].orientation.y),(-kpoints.getPoints()->pose[k].orientation.z),(-kpoints.getPoints()->pose[k].orientation.w));
// }
P_right_wrist.resize(3);
P_left_wrist.resize(3);
/* P_right_elbow.resize(6);
P_left_elbow.resize(6);
//Elbow Izquierdo
P_left_elbow[0] = P[1][0];
P_left_elbow[1] = P[1][1];
P_left_elbow[2] = P[1][2];
P_left_elbow[3] = Q_orientation[1][0];
P_left_elbow[4] = Q_orientation[1][1];
P_left_elbow[5] = Q_orientation[1][2];
//Left hand
P_left_wrist[0] = P[2][0];
P_left_wrist[1] = P[2][1];
P_left_wrist[2] = P[2][2];
P_left_wrist[3] = Q_orientation[2][0];
P_left_wrist[4] = Q_orientation[2][1];
P_left_wrist[5] = Q_orientation[2][2];
//Right elbow
P_right_elbow[0] = P[4][0];
P_right_elbow[1] = P[4][1];
P_right_elbow[2] = P[4][2];
P_right_elbow[3] = Q_orientation[4][0];
P_right_elbow[4] = Q_orientation[4][1];
P_right_elbow[5] = Q_orientation[4][2];
//Right hand
double x = 0.635163;
double y= -0.830166;
double z=0.320976;*/
P_right_wrist[0] = 0.935163;//P[5][0];
P_right_wrist[1] = -0.530166;//P[5][1];
P_right_wrist[2] = 0.820976; //P[5][2];
P_left_wrist[0] = 0.935163;//P[5][0];
P_left_wrist[1] = 0.530166;//P[5][1];
P_left_wrist[2] = 0.820976; //P[5][2];
//cout<<"hola"<<endl;
// P_right_wrist[3] = Q_orientation[5][0];
// P_right_wrist[4] = Q_orientation[5][1];
// P_right_wrist[5] = Q_orientation[5][2];
//kpoints.getPoints()->pose[k].position.x
// taskle->setDesiredValue(P_left_elbow);
//cout<<"P_left_elbow"<<endl;
//cout<<P_left_elbow.transpose()<<endl;
tasklh->setDesiredValue(P_left_wrist);
//cout<<"P_left_wrist"<<endl;
//cout<<P_left_wrist.transpose()<<endl;
// taskre->setDesiredValue(P_right_elbow);
//cout<<"P_right_elbow"<<endl;
//cout<<P_right_elbow.transpose()<<endl;
taskrh->setDesiredValue(P_right_wrist);
//cout<<"P_right_wrist"<<endl;
//cout<<P_right_wrist.transpose()<<endl;
//cout<<"KineSolver"<<endl;
solver.getPositionControl(qsensed, qdes);
//cout<<"publish"<<endl;
//cout<<"qdes:_"<<endl<<qdes<<endl;
for(int j=0; j<jnames.size();j++){
cout<<qdes[j]<<endl;
}
for(int j=0; j<jnames.size();j++){
jcmd.position[j]=qdes[j];
}
pub.publish(jcmd);
qsensed = qdes;
// for(int i=0; i<3; i++){
//cout<<i<<endl;
//cout<<i<<".-"<<jnames[i] <<": " <<endl;
/*
qposition(0,0)=P[3][0];
qposition(1,0)=P[3][1];
qposition(2,0)=P[3][2];
qposition(0,1)=P[4][0];
qposition(1,1)=P[4][1];
qposition(2,1)=P[4][2];
qposition(0,2)=P[5][0];
qposition(1,2)=P[5][1];
qposition(2,2)=P[5][2];
*/
/*
//}
marker_current_right_0->setPose(qposition.col(0));
marker_current_right_1->setPose(qposition.col(1));
marker_current_right_2->setPose(qposition.col(2) );
//marker_current_right_3->setPose(qdes[4] );
//marker_current_right_4->setPose(qdes[5] );
//marker_current_right_5->setPose(qdes[6] );
//marker_current_right_6->setPose(qdes[7] );
//marker_current_left_0->setPose(P[4]);
//marker_current_left_1->setPose(P[5]);
//marker_current_left_2->setPose(qdes[10] );
//marker_current_left_3->setPose(qdes[11] );
//marker_current_left_4->setPose(qdes[12] );
//marker_current_left_5->setPose(qdes[13] );
//marker_current_left_6->setPose(qdes[14] );
//marker_current_base->publish();
//marker_current_head->publish();
marker_current_right_0->publish();
marker_current_right_1->publish();
marker_current_right_2->publish();
//marker_current_right_3->publish();
//marker_current_right_4->publish();
//marker_current_right_5->publish();
//marker_current_right_6->publish();
//marker_current_left_0->publish();
//marker_current_left_1->publish();
//marker_current_left_2->publish();
//marker_current_left_3->publish();
//marker_current_left_4->publish();
//marker_current_left_5->publish();
//marker_current_left_6->publish();
}*/
ros::spinOnce();
rate.sleep();
}
return 0;
}
void RefreshAllIni() {
robot->RefreshIni();
}
int main(int argc, char** argv)
{
int i;
double th;
ros::init(argc, argv, "my_tf_broadcaster");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate rate(5.0);
// Visualization marker publisher
ros::Publisher vis_pub = node.advertise<visualization_msgs::Marker>( "visualization_marker", 100);
vis_pub_ptr = &vis_pub;
// Script tag publisher
ros::Publisher script_pub = node.advertise<cri::MarkerScript>( "visualization_marker_script", 100);
ros::Subscriber popSub = node.subscribe("pop", 10, popCallback);
cri::MarkerScript markerScript;
markerScript.ns = "";
markerScript.id = -1;
markerScript.action = cri::MarkerScript::ADD_OR_MODIFY;
markerScript.script_name = "Script1";
markerScript.trigger_event_type = cri::MarkerScript::LEFT_CLICK;
markerScript.filename = "package://cri/src/tutorials/technology_test/InitBubble.py";
// Why 3???
script_pub.publish(markerScript);
ros::spinOnce();
rate.sleep();
script_pub.publish(markerScript);
ros::spinOnce();
rate.sleep();
script_pub.publish(markerScript);
ros::spinOnce();
rate.sleep();
// Point cloud 2 publishers
ros::Publisher plainpc_pub = node.advertise<sensor_msgs::PointCloud2>( "plain_random_cloud", 20);
ros::Publisher rgbpc_pub = node.advertise<sensor_msgs::PointCloud2>( "rgb_random_cloud", 20);
ros::Publisher ls_pub = node.advertise<sensor_msgs::LaserScan>( "laser_scan_cloud", 20);
lastPoint = 0;
// Grid cells publisher
ros::Publisher gc_pub = node.advertise<nav_msgs::GridCells>( "grid_cells", 20);
// Occupancy grid publisher
ros::Publisher og_pub = node.advertise<nav_msgs::OccupancyGrid>( "occupancy_grid", 20);
// Path publisher
ros::Publisher path_pub = node.advertise<nav_msgs::Path>( "path_data", 20);
// Odometry publisher
ros::Publisher odo_pub = node.advertise<nav_msgs::Odometry>( "odo_data", 20);
// Pose publisher
ros::Publisher pose_pub = node.advertise<geometry_msgs::PoseStamped>( "pose_data", 20);
// PoseArray publisher
ros::Publisher pose_array_pub = node.advertise<geometry_msgs::PoseArray>( "pose_array_data", 20);
th = 0;
// Initialise bubble active marker demo
for (i = 0; i < BUBBLE_COUNT; i++)
{
bubbleX[i] = -15 - 15.0 * rand() * 1.0 / RAND_MAX;
bubbleY[i] = 15 + 15.0 * rand() * 1.0 / RAND_MAX;
bubbleHeight[i] = 0;
bubbleSpeed[i] = 0.02 * rand() * 1.0 / RAND_MAX + 0.01;
bubbleBurst[i] = 0;
}
#ifdef ROBOT
KDL::Tree tree;
// Find the CRI package directory
string packagePath = ros::package::getPath("cri");
string URDFFilename = packagePath + "/graphics/example_robot/urdf/wam.urdf";
if (!kdl_parser::treeFromFile(URDFFilename, tree))
{
ROS_ERROR("Failed to construct kdl tree");
return -1;
}
robot_state_publisher::RobotStatePublisher robotStatePub(tree);
std::map<std::string, double> robotConfig;
RobotModel * rm = new RobotModel();
string model = getTextFromFile(URDFFilename);
robotConfig["j1_joint"] = 0;
robotConfig["j2_joint"] = 0;
robotConfig["j3_joint"] = 0;
robotConfig["j4_joint"] = 0;
robotConfig["j5_joint"] = 0;
robotConfig["j6_joint"] = 0;
robotConfig["j7_joint"] = 0;
robotStatePub.publishFixedTransforms();
robotStatePub.publishTransforms(robotConfig, ros::Time::now());
#endif
int ensure = 5;
while (node.ok())
{
// Need a few frames
th += 0.1;
transform.setOrigin(tf::Vector3(25.0 + cos(th) * 5, 25.0, sin(th) * 5));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/pc2"));
transform.setOrigin(tf::Vector3(15.0, 15.0, 0));
transform.setRotation(tf::Quaternion(sin(th/2.0), 0, 0, cos(th/2.0)));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/scan"));
transform.setOrigin(tf::Vector3(25.0, -25.0,0));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/gridCells"));
transform.setOrigin(tf::Vector3(15.0, -15.0,0));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/occupancyGrid"));
transform.setOrigin(tf::Vector3(-15.0, 0, 1.5));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/robotHome"));
transform.setOrigin(tf::Vector3(-15.0, -15, 1.5));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/path"));
transform.setOrigin(tf::Vector3(-25.0, -25, 1.5));
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/world", "/odo"));
#ifdef ROBOT
robotStatePub.publishFixedTransforms();
robotStatePub.publishTransforms(robotConfig, ros::Time::now());
if (ensure > 0)
{
rm->go(node, model);
ensure--;
}
#endif
// Publish example markers
publishMarkerArrow(vis_pub, 0);
publishMarkerCube(vis_pub, 1);
publishMarkerSphere(vis_pub, 2);
publishMarkerCylinder(vis_pub, 3);
publishMarkerLineStrip(vis_pub, 4);
publishMarkerLineList(vis_pub, 5);
publishMarkerCubeList(vis_pub, 6);
publishMarkerSphereList(vis_pub, 7);
publishMarkerPoints(vis_pub, 8);
publishMarkerText(vis_pub, 9);
publishMarkerTriangle(vis_pub, 10);
publishMarkerMesh(vis_pub, 11);
// Publish example point clouds
publishPointCloud2(plainpc_pub);
publishRGBPointCloud2(rgbpc_pub);
publishLaserScan(ls_pub);
// Publish example grid cells
publishGridCells(gc_pub);
// Publish example occupancy grid
publishOccupancyGrid(og_pub);
// Publish example path
publishPath(path_pub);
// Publish example odometry
publishOdometry(odo_pub);
// Publish example pose
publishPoseStamped(pose_pub);
// Publish example pose array
publishPoseArray(pose_array_pub);
// Bubbles demo
publishBubbles(vis_pub, script_pub);
// Robot demo
publishRobotAndTable(vis_pub);
ros::spinOnce();
rate.sleep();
}
return 0;
};
qreal PhysicsEngineBase::wheelLinearSpeed(RobotModel &robot, const RobotModel::Wheel &wheel) const
{
return wheel.spoiledSpeed * 2 * mathUtils::pi * wheel.radius * robot.info().onePercentAngularVelocity() / 360;
}
