/**
*@brief 初期手先位置を設定
* @param s_p 初期手先位置
* @param the 初期手先姿勢
* @param maxSpeedCartesianTrans 手先の最大速度
* @param maxSpeedCartesianRot 手先姿勢の最大速度
* @param minTime 到達時間の最小値
*/
void TargetPos::setStartPoint(Vector3d s_p, double the, double maxSpeedCartesianTrans, double maxSpeedCartesianRot, double minTime)
{
start_pos(0) = s_p(0);
start_pos(1) = s_p(1);
start_pos(2) = s_p(2);
start_theta = the;
if(end_time <= 0)
{
double dx = target_pos(0)-start_pos[0];
double dy = target_pos(1)-start_pos[1];
double dz = target_pos(2)-start_pos[2];
double ST = sqrt(dx*dx+dy*dy+dz*dz);
end_time = ST/maxSpeedCartesianTrans;
double dt = target_theta-start_theta;
dt = sqrt(dt*dt)/maxSpeedCartesianRot;
if(end_time < dt)
end_time = dt;
if(end_time < minTime)
end_time = minTime;
std::cout << end_time << std::endl;
}
}
/**
*@brief 目標手先位置を設定
* @param t 到達時間(0以下に設定した場合は初期位置と目標位置との距離から自動的に計算)
* @param t_p 手先位置
* @param the 手先姿勢
*/
void TargetPos::setPoint(double t, Vector3d t_p, double the)
{
target_pos(0) = t_p(0);
target_pos(1) = t_p(1);
target_pos(2) = t_p(2);
target_theta = the;
type = Point;
end_time = t;
}
void WarkeyDlg::OnMotion(wxMouseEvent &evt) {
if (evt.LeftIsDown() && evt.Dragging()) {
wxPoint target_pos(evt.GetPosition());
auto delta = target_pos - orig_mouse_pos_;
orig_wnd_pos_ += delta;
Move(orig_wnd_pos_);
}
}
void SGSHero::onMoveTo(SGMessage* message)
{
int x, y;
message->getInt("x", &x);
message->getInt("y", &y);
SGSPoint target_pos(x,y);
SGSPointList& path = __terrain->calcShortestPath(this, target_pos);
if (path.empty()) {
log("Failed to find available path");
return;
} else {
path.erase(path.begin());
}
moveOneStep(&path);
}
int main () {
// file to write
ofstream outputfile("tracking.csv");
//make instance
double m_dt=0.001;
interpolator* i = new interpolator(1, m_dt,interpolator::HOFFARBIB,0.5);
double start=0.0,goal=1.0,goalv=0;
double test_time=10;
double t=0;
double x=0,v=0,a=0;
double rt;
//i->load("data.csv");
i->set(&start);
for (int j=0;j<test_time/m_dt;j++){
// set goal
double target,c_target;
double interpolation_time;
double interpolation_gain=10;
double Kp=0.1;
target=target_pos(t);
c_target=Kp*(target-x) +x;
interpolation_time= 0.1;//interpolation_gain*fabs(x-target);
//std::cout << fabs(x-target) << std::endl;
// std::cout << interpolation_time << std::endl;
i->setGoal(&c_target, interpolation_time, true);
// get interpolation val
if (i->isEmpty()) std::cout << "empty!" << std::endl;
i->get(&x,&v,&a,true);
outputfile << t << " " << x << " " << v << " "<< a << " " << target <<std::endl;
t+=i->deltaT();
}
outputfile.close();
return 0;
};
void Simu ::_make_robot_init(float duration)
{
robot_t rob = this->robot();
Eigen::Vector3d rot=rob->rot();
_arrival_angle= atan2( cos(rot[2])* sin(rot[1])* sin(rot[0]) + sin(rot[2])* cos(rot[0]), cos(rot[2])* cos(rot[1]))*180/M_PI;
//std::cout<<rot[0]<< " " << rot[1] <<" "<<rot[2]<<std::endl;
// std::cout<<"initial angle "<<_arrival_angle<<std::endl;
Eigen::Vector3d target_pos(0,2,0.2);
_controller.moveRobot(rob,0);
/* float t = 0.0f;
while (t < 1)
{
t += step;
next_step();
}*/
/*if(!stabilize_robot())
{
_energy=1e20;
_covered_distance=1e10;
std::cout<<"non stab"<<std::endl;
return;
}*/
Eigen::Vector3d prev_pos = rob->pos();
float t=0;
int index = 0;
#ifdef GRAPHIC
while (t < duration && !_visitor.done())
#else
while (t < duration)
#endif
{
_controller.moveRobot(rob,t);
if(_robot->bodies()[0]->get_in_contact() || _env->get_colision_between_legs() || _robot->pos()[2]>0.5)
{
#ifdef GRAPHIC
std::cout<<"mort subite"<<std::endl;
#endif
_covered_distance=-10002;
return;
}
int nbCassee=0;
if (index%2==0)
for (unsigned i = 0; i < 6; ++i)
{
switch (i)
{
case 0:
if (_controller.isBroken(i))
{
_behavior_contact_0.push_back(0);
nbCassee++;
}
else
{
_behavior_contact_0.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() );
}
break;
case 1:
if (_controller.isBroken(i))
{
_behavior_contact_1.push_back(0);
nbCassee++;
}
else
{
_behavior_contact_1.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() );
}
break;
case 2:
if (_controller.isBroken(i))
{
_behavior_contact_2.push_back(0);
nbCassee++;
}
else
{
_behavior_contact_2.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() );
}
break;
case 3:
if (_controller.isBroken(i))
{
_behavior_contact_3.push_back(0);
nbCassee++;
}
else
{
_behavior_contact_3.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() );
}
break;
case 4:
if (_controller.isBroken(i))
{
_behavior_contact_4.push_back(0);
nbCassee++;
}
else
{
_behavior_contact_4.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() );
}
break;
case 5:
if (_controller.isBroken(i))
{
_behavior_contact_5.push_back(0);
nbCassee++;
}
else
{
_behavior_contact_5.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() );
}
break;
}
}
_behavior_traj.push_back(rob->pos());
t += step;
next_step();
++index;
}
Eigen::Vector3d end_mov_pos = rob->pos();
//std::cout<<"fin mov:"<<end_mov_pos<<std::endl;
stabilize_robot();
Eigen::Vector3d end_stab_pos = rob->pos();
//std::cout<<"fin stab:"<<end_stab_pos<<std::endl;
if((end_stab_pos-end_mov_pos).norm()>0.2)
{
_covered_distance=-10002;
return;
}
// _covered_distance=sqrt((next_pos[0] - target_pos[0])*(next_pos[0] - target_pos[0])+(next_pos[1] - target_pos[1])*(next_pos[1] - target_pos[1])+(next_pos[2] - target_pos[2])*(next_pos[2] - target_pos[2]));
// _covered_distance=2-_covered_distance;
// _covered_distance = round(next_pos[1]*100) / 100.0f;
_final_pos.resize(2);
_final_pos[0]=end_stab_pos[0]-prev_pos[0];
_final_pos[1]=end_stab_pos[1]-prev_pos[1];
// std::cout<<"final pos:"<<_final_pos[0]<<" "<< _final_pos[1]<<std::endl;
// _covered_distance = round(sqrt(next_pos[0]*next_pos[0]+next_pos[1]*next_pos[1]+next_pos[2]*next_pos[2])*100) / 100.0f;
_covered_distance = round(_final_pos[1]*100) / 100.0f;
}
//-----------------------------------------------------------------------------
// render()
//-----------------------------------------------------------------------------
BOOL LLImagePreviewSculpted::render()
{
mNeedsUpdate = FALSE;
LLGLSUIDefault def;
LLGLDisable no_blend(GL_BLEND);
LLGLEnable cull(GL_CULL_FACE);
LLGLDepthTest depth(GL_TRUE);
glMatrixMode(GL_PROJECTION);
gGL.pushMatrix();
glLoadIdentity();
glOrtho(0.0f, mWidth, 0.0f, mHeight, -1.0f, 1.0f);
glMatrixMode(GL_MODELVIEW);
gGL.pushMatrix();
glLoadIdentity();
gGL.color4f(0.15f, 0.2f, 0.3f, 1.f);
gl_rect_2d_simple( mWidth, mHeight );
glMatrixMode(GL_PROJECTION);
gGL.popMatrix();
glMatrixMode(GL_MODELVIEW);
gGL.popMatrix();
glClear(GL_DEPTH_BUFFER_BIT);
LLVector3 target_pos(0, 0, 0);
LLQuaternion camera_rot = LLQuaternion(mCameraPitch, LLVector3::y_axis) *
LLQuaternion(mCameraYaw, LLVector3::z_axis);
LLQuaternion av_rot = camera_rot;
LLViewerCamera::getInstance()->setOriginAndLookAt(
target_pos + ((LLVector3(mCameraDistance, 0.f, 0.f) + mCameraOffset) * av_rot), // camera
LLVector3::z_axis, // up
target_pos + (mCameraOffset * av_rot) ); // point of interest
stop_glerror();
LLViewerCamera::getInstance()->setAspect((F32) mWidth / mHeight);
LLViewerCamera::getInstance()->setView(LLViewerCamera::getInstance()->getDefaultFOV() / mCameraZoom);
LLViewerCamera::getInstance()->setPerspective(FALSE, mOrigin.mX, mOrigin.mY, mWidth, mHeight, FALSE);
const LLVolumeFace &vf = mVolume->getVolumeFace(0);
U32 num_indices = vf.mIndices.size();
mVertexBuffer->setBuffer(LLVertexBuffer::MAP_VERTEX | LLVertexBuffer::MAP_NORMAL);
gPipeline.enableLightsAvatar();
gGL.pushMatrix();
const F32 SCALE = 1.25f;
gGL.scalef(SCALE, SCALE, SCALE);
const F32 BRIGHTNESS = 0.9f;
gGL.color3f(BRIGHTNESS, BRIGHTNESS, BRIGHTNESS);
mVertexBuffer->draw(LLRender::TRIANGLES, num_indices, 0);
gGL.popMatrix();
return TRUE;
}
void Simu::_make_robot_init(float duration)
{
robot_t rob = this->robot();
Eigen::Vector3d rot = rob->rot();
_arrival_angle = atan2(cos(rot[2]) * sin(rot[1]) * sin(rot[0]) + sin(rot[2]) * cos(rot[0]), cos(rot[2]) * cos(rot[1])) * 180 / M_PI;
//std::cout<<rot[0]<< " " << rot[1] <<" "<<rot[2]<<std::endl;
// std::cout<<"initial angle "<<_arrival_angle<<std::endl;
Eigen::Vector3d target_pos(0, 2, 0.2);
_controller.moveRobot(rob, 0);
/* float t = 0.0f;
while (t < 1)
{
t += step;
next_step();
}*/
/*if(!stabilize_robot())
{
_energy=1e20;
_covered_distance=1e10;
std::cout<<"non stab"<<std::endl;
return;
}*/
Eigen::VectorXd act_state = _get_state(rob);
float t = 0;
int index = 0;
#ifdef GRAPHIC
while (t < duration && !_visitor.done())
#else
while (t < duration)
#endif
{
_controller.moveRobot(rob, t);
Eigen::VectorXd new_state = _get_state(rob);
_energy += (act_state - new_state).array().abs().sum();
act_state = new_state;
if (_robot->bodies()[0]->get_in_contact() || _env->get_colision_between_legs()) {
#ifdef GRAPHIC
std::cout << "mort subite" << std::endl;
#endif
std::cout << "mort subite" << std::endl;
//Eigen::Vector3d next_pos = rob->pos();
//_covered_distance = round(next_pos[1]*100) / 100.0f;
_covered_distance = 00.0f;
//_covered_distance=-10002;
return;
}
int nbCassee = 0;
if (index % 2 == 0)
for (unsigned i = 0; i < 6; ++i) {
switch (i) {
case 0:
if (_controller.isBroken(i)) {
_behavior_contact_0.push_back(0);
nbCassee++;
}
else {
_behavior_contact_0.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 1:
if (_controller.isBroken(i)) {
_behavior_contact_1.push_back(0);
nbCassee++;
}
else {
_behavior_contact_1.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 2:
if (_controller.isBroken(i)) {
_behavior_contact_2.push_back(0);
nbCassee++;
}
else {
_behavior_contact_2.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 3:
if (_controller.isBroken(i)) {
_behavior_contact_3.push_back(0);
nbCassee++;
}
else {
_behavior_contact_3.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 4:
if (_controller.isBroken(i)) {
_behavior_contact_4.push_back(0);
nbCassee++;
}
else {
_behavior_contact_4.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
case 5:
if (_controller.isBroken(i)) {
_behavior_contact_5.push_back(0);
nbCassee++;
}
else {
_behavior_contact_5.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact());
}
break;
}
}
_behavior_traj.push_back(rob->pos());
t += step;
next_step();
++index;
}
if (fabs(_angle) < 0.01) {
stabilize_robot();
}
Eigen::Vector3d next_pos = rob->pos();
// _covered_distance=sqrt((next_pos[0] - target_pos[0])*(next_pos[0] - target_pos[0])+(next_pos[1] - target_pos[1])*(next_pos[1] - target_pos[1])+(next_pos[2] - target_pos[2])*(next_pos[2] - target_pos[2]));
// _covered_distance=2-_covered_distance;
// _covered_distance = round(next_pos[1]*100) / 100.0f;
_final_pos.resize(2);
_final_pos[0] = next_pos[0];
_final_pos[1] = next_pos[1];
// _covered_distance = round(sqrt(next_pos[0]*next_pos[0]+next_pos[1]*next_pos[1]+next_pos[2]*next_pos[2])*100) / 100.0f;
_covered_distance = round(next_pos[1] * 100) / 100.0f;
if (fabs(_covered_distance) > 10) {
_covered_distance = 00.0f;
}
_direction = atan2(-next_pos[0], next_pos[1]) * 180 / M_PI;
rot = rob->rot();
_arrival_angle = atan2(cos(rot[2]) * sin(rot[1]) * sin(rot[0]) + sin(rot[2]) * cos(rot[0]), cos(rot[2]) * cos(rot[1])) * 180 / M_PI;
while (_arrival_angle < -180)
_arrival_angle += 360;
while (_arrival_angle > 180)
_arrival_angle -= 360;
//std::cout<<"final angle "<<_arrival_angle<<" déviation: "<<_direction-_arrival_angle<<" fit: "<<_covered_distance<<std::endl;
//std::cout<<rot[0]<< " " << rot[1] <<" "<<rot[2]<<std::endl;
if (_transf) {
std::vector<std::vector<float>> contacts;
contacts.push_back(_behavior_contact_0);
contacts.push_back(_behavior_contact_1);
contacts.push_back(_behavior_contact_2);
contacts.push_back(_behavior_contact_3);
contacts.push_back(_behavior_contact_4);
contacts.push_back(_behavior_contact_5);
write_data(contacts, "contacts");
std::vector<float> angles;
angles.push_back(rob->rot()[0]);
angles.push_back(rob->rot()[1]);
angles.push_back(rob->rot()[2]);
write_data(angles, "angles");
write_data(_covered_distance, "fit");
}
}
void DelaunayPulseIn::update( float elapsed )
{
timer += elapsed;
// update animation
//printf("queue contains: ");
vector<MovingPulse> dequeued;
while ( !queued_pulses.empty() && queued_pulses.top().timer < timer )
{
// dequeue
dequeued.push_back( queued_pulses.top() );
// remove from queue
queued_pulses.pop();
}
// pings
vector<pair< int, float > > pings;
// coalesce pulses for the same node
map<int,int> coalesced;
for ( int i=0; i<dequeued.size(); i++ )
{
int id = dequeued[i].id;
// add ping here (because we might drop it later)
pings.push_back( make_pair( id, dequeued[i].energy ) );
// coalesce
if ( coalesced.find( id ) == coalesced.end() )
{
coalesced[id] = i;
}
else
{
// merge
dequeued[coalesced[id]].energy += dequeued[i].energy;
// remove
dequeued.erase( dequeued.begin()+i );
i--;
}
}
// no go through coalesced
for ( int i=0; i<dequeued.size(); i++ )
{
// fetch id
int curr = dequeued[i].id;
float energy = dequeued[i].energy;
//printf("%2i:%7.5f ", curr, energy );
// pulse the led
if ( dequeued[i].timer > 0 )
lights->pulse( curr, energy );
// finished?
if ( curr == target_index || energy < 0.001f )
continue;
// add the closest neighbours to queue
set<int> adj = delaunay->getNeighbours( curr );
ofxVec2f curr_pos( lights->getLight( curr ).getX(),
lights->getLight( curr ).getY() );
ofxVec2f target_pos( lights->getLight( target_index ).getX(),
lights->getLight( target_index ).getY() );
// use for calculating dot product (-> angle )
ofxVec2f target_delta = target_pos - curr_pos;
float distance_to_target = target_delta.length();
target_delta /= distance_to_target;
//printf("delta to target is %7.5f %7.5f\n", target_delta.x, target_delta.y );
for ( set<int>::iterator jt = adj.begin();
jt != adj.end();
++jt )
{
// add neighbours, distributing energy
ofxVec2f adj_pos( lights->getLight( *jt ).getX(),
lights->getLight( *jt ).getY() );
ofxVec2f adj_delta = adj_pos - curr_pos;
float distance = adj_delta.length();
adj_delta /= distance;
// dot product
float direction_accuracy = target_delta.dot( adj_delta );
// would this path take us towards the target?
//printf(" adj delta to next is %7.5f %f7.5 -> accuracy %7.5f\n", adj_delta.x, adj_delta.y, direction_accuracy );
if ( direction_accuracy > 0.5f )
{
direction_accuracy*=direction_accuracy;
// yes
//float new_energy = direction_accuracy*0.5f*(max_brightness*(1.0f-(distance_to_target/initial_radius)));
float falloff = 0.25f;
float new_energy = energy*direction_accuracy*0.5f - energy*distance*falloff;
queued_pulses.push( MovingPulse(*jt, new_energy, ofRandom(0.8f,1.2f)*(timer+(distance/speed)) ) );
}
}
}
//printf("\n");
// send pings
for ( int i=0; i<pings.size(); i++ )
{
ofxOscMessage m;
m.setAddress( "/delaunay/ping" );
m.addFloatArg( pings[i].second );
const Light& light = lights->getLight( pings[i].first );
m.addFloatArg( light.getX() );
m.addFloatArg( light.getY() );
m.addFloatArg( delaunay->getId() );
Osc::getInstance()->sendMessage( m );
}
}
