void
filter_calc_coeffs (Filter *f, float freq, float q)
{
// temp coef vars
// limit freq and q for not getting bad noise out of the filter...
freq = q_max(freq, MIN_FREQ);
q = q_max(q, MIN_Q);
switch (f->type) {
case FILTER_LOWPASS_RC12:
case FILTER_BANDPASS_RC12:
case FILTER_HIGHPASS_RC12:
case FILTER_LOWPASS_RC24:
case FILTER_BANDPASS_RC24:
case FILTER_HIGHPASS_RC24:
filter_calc_rc_coeffs(&f->c.r, freq, q, f->sample_rate);
break;
case FILTER_FORMANTFILTER:
filter_calc_formant_coeffs(&f->c.f, freq, q, f->sample_rate);
break;
case FILTER_MOOG:
filter_calc_moog_coeffs(&f->c.m, freq, q, f->sample_rate);
break;
default:
filter_calc_basic_coeffs(&f->c.b, f->type, freq, q, f->sample_rate);
break;
}
}
int ArmKdlInverseKinematics::CartToJnt(const KDL::JntArray& q_init,
const KDL::Frame& p_in,
KDL::JntArray& q_out)
{
KDL::JntArray q_min(_min_angles.size());
KDL::JntArray q_max(_max_angles.size());
// copy the joint limits to a KDL array
for (unsigned int i = 0; i < _min_angles.size(); i++) {
q_min(i) = _min_angles[i];
q_max(i) = _max_angles[i];
}
// Forward position solver
KDL::ChainFkSolverPos_recursive fksolver1(_chain);
// Inverse velocity solver
KDL::ChainIkSolverVel_pinv iksolver1v(_chain);
// Maximum 100 iterations, stop at accuracy 1e-6
KDL::ChainIkSolverPos_NR_JL iksolverpos(_chain, q_min, q_max, fksolver1, iksolver1v, 1000, 1e-6);
int ret = iksolverpos.CartToJnt(q_init, p_in, q_out);
ROS_DEBUG("q_init: %f %f %f %f %f", q_init(0), q_init(1), q_init(2), q_init(3), q_init(4));
ROS_DEBUG("q_out: %f %f %f %f %f", q_out(0), q_out(1), q_out(2), q_out(3), q_out(4));
if (ret < 0) {
ROS_DEBUG("Inverse Kinematic found no solution. KDL Return value = %i", ret);
return -1;
} else {
ROS_DEBUG("Inverse Kinematic found a solution");
return 1;
}
}
/*
=============
Fog_FogCommand_f
handle the 'fog' console command
=============
*/
void Fog_FogCommand_f (void)
{
switch (Cmd_Argc())
{
default:
case 1:
Con_Printf("usage:\n");
Con_Printf(" fog <density>\n");
Con_Printf(" fog <red> <green> <blue>\n");
Con_Printf(" fog <density> <red> <green> <blue>\n");
Con_Printf("current values:\n");
Con_Printf(" \"density\" is \"%f\"\n", fog_density);
Con_Printf(" \"red\" is \"%f\"\n", fog_red);
Con_Printf(" \"green\" is \"%f\"\n", fog_green);
Con_Printf(" \"blue\" is \"%f\"\n", fog_blue);
break;
case 2:
Fog_Update(q_max(0.0, atof(Cmd_Argv(1))),
fog_red,
fog_green,
fog_blue,
0.0);
break;
case 3: //TEST
Fog_Update(q_max(0.0, atof(Cmd_Argv(1))),
fog_red,
fog_green,
fog_blue,
atof(Cmd_Argv(2)));
break;
case 4:
Fog_Update(fog_density,
CLAMP(0.0, atof(Cmd_Argv(1)), 1.0),
CLAMP(0.0, atof(Cmd_Argv(2)), 1.0),
CLAMP(0.0, atof(Cmd_Argv(3)), 1.0),
0.0);
break;
case 5:
Fog_Update(fmax(0.0, atof(Cmd_Argv(1))),
CLAMP(0.0, atof(Cmd_Argv(2)), 1.0),
CLAMP(0.0, atof(Cmd_Argv(3)), 1.0),
CLAMP(0.0, atof(Cmd_Argv(4)), 1.0),
0.0);
break;
case 6: //TEST
Fog_Update(fmax(0.0, atof(Cmd_Argv(1))),
CLAMP(0.0, atof(Cmd_Argv(2)), 1.0),
CLAMP(0.0, atof(Cmd_Argv(3)), 1.0),
CLAMP(0.0, atof(Cmd_Argv(4)), 1.0),
atof(Cmd_Argv(5)));
break;
}
}
/*
================
IN_ApplyMoveEasing
clamps coordinates to a square with coordinates +/- sqrt(2)/2, then scales them to +/- 1.
This wastes a bit of stick range, but gives the diagonals coordinates of (+/-1,+/-1),
so holding the stick on a diagonal gives the same speed boost as holding the forward and strafe keyboard keys.
================
*/
static joyaxis_t IN_ApplyMoveEasing(joyaxis_t axis)
{
joyaxis_t result = {0};
const float v = sqrtf(2.0f) / 2.0f;
result.x = q_max(-v, q_min(v, axis.x));
result.y = q_max(-v, q_min(v, axis.y));
result.x /= v;
result.y /= v;
return result;
}
/*
=============
Fog_ParseServerMessage
handle an SVC_FOG message from server
=============
*/
void Fog_ParseServerMessage (void)
{
float density, red, green, blue, time;
density = MSG_ReadByte() / 255.0;
red = MSG_ReadByte() / 255.0;
green = MSG_ReadByte() / 255.0;
blue = MSG_ReadByte() / 255.0;
time = q_max(0.0, MSG_ReadShort() / 100.0);
Fog_Update (density, red, green, blue, time);
}
interval mul_ii(interval x, interval y) /* [x] * [y] */
{ interval res;
double h;
if (x.INF >=0) { /* 0 <= [x] */
if (y.INF >=0) { /* 0 <= [y] */
h=x.INF*y.INF;
res.INF=(h==0 ? 0 : q_pred(h));
} else { /* [y] <= 0 or 0 \in [y] */
h=x.SUP*y.INF;
res.INF=(x.SUP==0 ? 0 : q_pred(h));
}
if (y.SUP <=0) { /* [y] <= 0 */
h=x.INF*y.SUP;
res.SUP=(h==0 ? 0 : q_succ(h));
} else { /* 0 <= [y] or 0 \in [y] */
h=x.SUP*y.SUP;
res.SUP=(x.SUP==0 ? 0 : q_succ(h));
}
} else if (x.SUP<=0) { /* [x] <= 0 */
if (y.SUP<=0) { /* [y] <= 0 */
h=x.SUP*y.SUP;
res.INF=(h==0 ? 0 : q_pred(h));
} else /* 0 <= [y] or 0 \in [y] */
res.INF=q_pred(x.INF*y.SUP);
if (y.INF>=0) { /* 0 <= [y] */
h=x.SUP*y.INF;
res.SUP=(h==0 ? 0 : q_succ(h));
} else /* [y] <= 0 or 0 \in [y] */
res.SUP=q_succ(x.INF*y.INF);
} else { /* 0 \in [x] */
if (y.INF>=0) { /* 0 <= [y] */
res.INF=q_pred(x.INF*y.SUP);
res.SUP=q_succ(x.SUP*y.SUP);
} else if (y.SUP<=0) { /* [y] <= 0 */
res.INF=q_pred(x.SUP*y.INF);
res.SUP=q_succ(x.INF*y.INF);
} else { /* 0 \in [x], 0 \in [y] */
res.INF=q_pred( q_min(x.INF*y.SUP, x.SUP*y.INF) );
res.SUP=q_succ( q_max(x.INF*y.INF, x.SUP*y.SUP) );
}
}
return res;
}
bool ChainIkSolverPos_LMA_JL_Mimic::obeysLimits(const KDL::JntArray &q_out)
{
bool obeys_limits = true;
for(size_t i = 0; i < chain.getNrOfJoints(); i++)
{
if( (q_out(i) < (q_min(i)-0.0001)) || (q_out(i) > (q_max(i)+0.0001)) )
{
// One element of solution is not within limits
obeys_limits = false;
break;
}
}
return obeys_limits;
}
/*
=================
SCR_CalcRefdef
Must be called whenever vid changes
Internal use only
=================
*/
static void SCR_CalcRefdef (void)
{
float size, scale; //johnfitz -- scale
// force the status bar to redraw
Sbar_Changed ();
scr_tileclear_updates = 0; //johnfitz
// bound viewsize
if (scr_viewsize.value < 30)
Cvar_SetQuick (&scr_viewsize, "30");
if (scr_viewsize.value > 120)
Cvar_SetQuick (&scr_viewsize, "120");
// bound fov
if (scr_fov.value < 10)
Cvar_SetQuick (&scr_fov, "10");
if (scr_fov.value > 170)
Cvar_SetQuick (&scr_fov, "170");
vid.recalc_refdef = 0;
//johnfitz -- rewrote this section
size = scr_viewsize.value;
scale = CLAMP (1.0, scr_sbarscale.value, (float)glwidth / 320.0);
if (size >= 120 || cl.intermission || scr_sbaralpha.value < 1) //johnfitz -- scr_sbaralpha.value
sb_lines = 0;
else if (size >= 110)
sb_lines = 24 * scale;
else
sb_lines = 48 * scale;
size = q_min(scr_viewsize.value, 100) / 100;
//johnfitz
//johnfitz -- rewrote this section
r_refdef.vrect.width = q_max(glwidth * size, 96); //no smaller than 96, for icons
r_refdef.vrect.height = q_min(glheight * size, glheight - sb_lines); //make room for sbar
r_refdef.vrect.x = (glwidth - r_refdef.vrect.width)/2;
r_refdef.vrect.y = (glheight - sb_lines - r_refdef.vrect.height)/2;
//johnfitz
r_refdef.fov_x = AdaptFovx(scr_fov.value, vid.width, vid.height);
r_refdef.fov_y = CalcFovy (r_refdef.fov_x, r_refdef.vrect.width, r_refdef.vrect.height);
scr_vrect = r_refdef.vrect;
}
TorqueEstimationTree::TorqueEstimationTree(KDL::Tree& icub_kdl,
std::vector< kdl_format_io::FTSensorData > ft_sensors,
std::vector< std::string > ft_serialization,
std::string fixed_link, unsigned int verbose)
{
yarp::sig::Vector q_max(icub_kdl.getNrOfJoints(),1000.0);
yarp::sig::Vector q_min(icub_kdl.getNrOfJoints(),-1000.0);
KDL::CoDyCo::TreeSerialization serial(icub_kdl);
std::vector<std::string> dof_serialization;
for(int i = 0; i < serial.getNrOfDOFs(); i++ )
{
dof_serialization.push_back(serial.getDOFName(i));
}
TorqueEstimationConstructor(icub_kdl,ft_sensors,dof_serialization,ft_serialization,q_min,q_max,fixed_link,verbose);
}
/*
==================
Host_ServerFrame
==================
*/
void Host_ServerFrame (void)
{
int i, active; //johnfitz
edict_t *ent; //johnfitz
// run the world state
pr_global_struct->frametime = host_frametime;
// set the time and clear the general datagram
SV_ClearDatagram ();
// check for new clients
SV_CheckForNewClients ();
// read client messages
SV_RunClients ();
// move things around and think
// always pause in single player if in console or menus
if (!sv.paused && (svs.maxclients > 1 || key_dest == key_game) )
SV_Physics ();
//johnfitz -- devstats
if (cls.signon == SIGNONS)
{
for (i=0, active=0; i<sv.num_edicts; i++)
{
ent = EDICT_NUM(i);
if (!ent->free)
active++;
}
if (active > 600 && dev_peakstats.edicts <= 600)
Con_DWarning ("%i edicts exceeds standard limit of 600.\n", active);
dev_stats.edicts = active;
dev_peakstats.edicts = q_max(active, dev_peakstats.edicts);
}
//johnfitz
// send all messages to the clients
SV_SendClientMessages ();
}
/*
=====================
CL_ParseServerMessage
=====================
*/
void CL_ParseServerMessage (void)
{
int cmd;
int i;
const char *str; //johnfitz
int total, j, lastcmd; //johnfitz
//
// if recording demos, copy the message out
//
if (cl_shownet.value == 1)
Con_Printf ("%i ",net_message.cursize);
else if (cl_shownet.value == 2)
Con_Printf ("------------------\n");
cl.onground = false; // unless the server says otherwise
//
// parse the message
//
MSG_BeginReading ();
lastcmd = 0;
while (1)
{
if (msg_badread)
Host_Error ("CL_ParseServerMessage: Bad server message");
cmd = MSG_ReadByte ();
if (cmd == -1)
{
SHOWNET("END OF MESSAGE");
return; // end of message
}
// if the high bit of the command byte is set, it is a fast update
if (cmd & U_SIGNAL) //johnfitz -- was 128, changed for clarity
{
SHOWNET("fast update");
CL_ParseUpdate (cmd&127);
continue;
}
SHOWNET(svc_strings[cmd]);
// other commands
switch (cmd)
{
default:
Host_Error ("Illegible server message, previous was %s\n", svc_strings[lastcmd]); //johnfitz -- added svc_strings[lastcmd]
break;
case svc_nop:
// Con_Printf ("svc_nop\n");
break;
case svc_time:
cl.mtime[1] = cl.mtime[0];
cl.mtime[0] = MSG_ReadFloat ();
break;
case svc_clientdata:
CL_ParseClientdata (); //johnfitz -- removed bits parameter, we will read this inside CL_ParseClientdata()
break;
case svc_version:
i = MSG_ReadLong ();
//johnfitz -- support multiple protocols
if (i != PROTOCOL_NETQUAKE && i != PROTOCOL_FITZQUAKE)
Host_Error ("Server returned version %i, not %i or %i\n", i, PROTOCOL_NETQUAKE, PROTOCOL_FITZQUAKE);
cl.protocol = i;
//johnfitz
break;
case svc_disconnect:
Host_EndGame ("Server disconnected\n");
case svc_print:
Con_Printf ("%s", MSG_ReadString ());
break;
case svc_centerprint:
//johnfitz -- log centerprints to console
str = MSG_ReadString ();
SCR_CenterPrint (str);
Con_LogCenterPrint (str);
//johnfitz
break;
case svc_stufftext:
cls.stufftext_frame = host_framecount; // allow full frame update
// in demo playback -- Pa3PyX
Cbuf_AddText (MSG_ReadString ());
break;
case svc_damage:
V_ParseDamage ();
break;
case svc_serverinfo:
CL_ParseServerInfo ();
vid.recalc_refdef = true; // leave intermission full screen
break;
case svc_setangle:
for (i=0 ; i<3 ; i++)
cl.viewangles[i] = MSG_ReadAngle ();
break;
case svc_setview:
cl.viewentity = MSG_ReadShort ();
break;
case svc_lightstyle:
i = MSG_ReadByte ();
if (i >= MAX_LIGHTSTYLES)
Sys_Error ("svc_lightstyle > MAX_LIGHTSTYLES");
q_strlcpy (cl_lightstyle[i].map, MSG_ReadString(), MAX_STYLESTRING);
cl_lightstyle[i].length = Q_strlen(cl_lightstyle[i].map);
//johnfitz -- save extra info
if (cl_lightstyle[i].length)
{
total = 0;
cl_lightstyle[i].peak = 'a';
for (j=0; j<cl_lightstyle[i].length; j++)
{
total += cl_lightstyle[i].map[j] - 'a';
cl_lightstyle[i].peak = q_max(cl_lightstyle[i].peak, cl_lightstyle[i].map[j]);
}
cl_lightstyle[i].average = total / cl_lightstyle[i].length + 'a';
}
else
cl_lightstyle[i].average = cl_lightstyle[i].peak = 'm';
//johnfitz
break;
case svc_sound:
CL_ParseStartSoundPacket();
break;
case svc_stopsound:
i = MSG_ReadShort();
S_StopSound(i>>3, i&7);
break;
case svc_updatename:
Sbar_Changed ();
i = MSG_ReadByte ();
if (i >= cl.maxclients)
Host_Error ("CL_ParseServerMessage: svc_updatename > MAX_SCOREBOARD");
q_strlcpy (cl.scores[i].name, MSG_ReadString(), MAX_SCOREBOARDNAME);
break;
case svc_updatefrags:
Sbar_Changed ();
i = MSG_ReadByte ();
if (i >= cl.maxclients)
Host_Error ("CL_ParseServerMessage: svc_updatefrags > MAX_SCOREBOARD");
cl.scores[i].frags = MSG_ReadShort ();
break;
case svc_updatecolors:
Sbar_Changed ();
i = MSG_ReadByte ();
if (i >= cl.maxclients)
Host_Error ("CL_ParseServerMessage: svc_updatecolors > MAX_SCOREBOARD");
cl.scores[i].colors = MSG_ReadByte ();
CL_NewTranslation (i);
break;
case svc_particle:
R_ParseParticleEffect ();
break;
case svc_spawnbaseline:
i = MSG_ReadShort ();
// must use CL_EntityNum() to force cl.num_entities up
CL_ParseBaseline (CL_EntityNum(i), 1); // johnfitz -- added second parameter
break;
case svc_spawnstatic:
CL_ParseStatic (1); //johnfitz -- added parameter
break;
case svc_temp_entity:
CL_ParseTEnt ();
break;
case svc_setpause:
cl.paused = MSG_ReadByte ();
if (cl.paused)
{
CDAudio_Pause ();
BGM_Pause ();
}
else
{
CDAudio_Resume ();
BGM_Resume ();
}
break;
case svc_signonnum:
i = MSG_ReadByte ();
if (i <= cls.signon)
Host_Error ("Received signon %i when at %i", i, cls.signon);
cls.signon = i;
//johnfitz -- if signonnum==2, signon packet has been fully parsed, so check for excessive static ents and efrags
if (i == 2)
{
if (cl.num_statics > 128)
Con_Warning ("%i static entities exceeds standard limit of 128.\n", cl.num_statics);
R_CheckEfrags ();
}
//johnfitz
CL_SignonReply ();
break;
case svc_killedmonster:
cl.stats[STAT_MONSTERS]++;
break;
case svc_foundsecret:
cl.stats[STAT_SECRETS]++;
break;
case svc_updatestat:
i = MSG_ReadByte ();
if (i < 0 || i >= MAX_CL_STATS)
Sys_Error ("svc_updatestat: %i is invalid", i);
cl.stats[i] = MSG_ReadLong ();;
break;
case svc_spawnstaticsound:
CL_ParseStaticSound (1); //johnfitz -- added parameter
break;
case svc_cdtrack:
cl.cdtrack = MSG_ReadByte ();
cl.looptrack = MSG_ReadByte ();
if ( (cls.demoplayback || cls.demorecording) && (cls.forcetrack != -1) )
BGM_PlayCDtrack ((byte)cls.forcetrack, true);
else
BGM_PlayCDtrack ((byte)cl.cdtrack, true);
break;
case svc_intermission:
cl.intermission = 1;
cl.completed_time = cl.time;
vid.recalc_refdef = true; // go to full screen
break;
case svc_finale:
cl.intermission = 2;
cl.completed_time = cl.time;
vid.recalc_refdef = true; // go to full screen
//johnfitz -- log centerprints to console
str = MSG_ReadString ();
SCR_CenterPrint (str);
Con_LogCenterPrint (str);
//johnfitz
break;
case svc_cutscene:
cl.intermission = 3;
cl.completed_time = cl.time;
vid.recalc_refdef = true; // go to full screen
//johnfitz -- log centerprints to console
str = MSG_ReadString ();
SCR_CenterPrint (str);
Con_LogCenterPrint (str);
//johnfitz
break;
case svc_sellscreen:
Cmd_ExecuteString ("help", src_command);
break;
//johnfitz -- new svc types
case svc_skybox:
Sky_LoadSkyBox (MSG_ReadString());
break;
case svc_bf:
Cmd_ExecuteString ("bf", src_command);
break;
case svc_fog:
Fog_ParseServerMessage ();
break;
case svc_spawnbaseline2: //PROTOCOL_FITZQUAKE
i = MSG_ReadShort ();
// must use CL_EntityNum() to force cl.num_entities up
CL_ParseBaseline (CL_EntityNum(i), 2);
break;
case svc_spawnstatic2: //PROTOCOL_FITZQUAKE
CL_ParseStatic (2);
break;
case svc_spawnstaticsound2: //PROTOCOL_FITZQUAKE
CL_ParseStaticSound (2);
break;
//johnfitz
}
lastcmd = cmd; //johnfitz
}
}
bool unigripper_motoman_plant_t::IK_solver(const config_t& effector_config, space_point_t* computed_state, bool set_grasping, const space_point_t* seed_state, bool do_min)
{
double qx,qy,qz,qw;
double x,y,z;
effector_config.get_orientation().get(qx,qy,qz,qw);
effector_config.get_position(x,y,z);
KDL::Chain chain1;
my_tree->getChain("base_link","vacuum_volume",chain1);
KDL::JntArray q(chain1.getNrOfJoints());
KDL::JntArray q_out(chain1.getNrOfJoints());
KDL::JntArray q_min(chain1.getNrOfJoints());
KDL::JntArray q_max(chain1.getNrOfJoints());
std::vector< double > state_var;
if( seed_state != NULL )
state_space->copy_point_to_vector( seed_state, state_var );
else
state_space->copy_to_vector( state_var );
q(0) = state_var[0];
q(1) = state_var[1];
q(2) = state_var[2];
q(3) = state_var[3];
q(4) = state_var[4];
q(5) = state_var[5];
q(6) = state_var[6];
q(7) = state_var[7];
q(8) = state_var[15];
q_min(0) = state_space->get_bounds()[0]->get_lower_bound();
q_min(1) = state_space->get_bounds()[1]->get_lower_bound();
q_min(2) = state_space->get_bounds()[2]->get_lower_bound();
q_min(3) = state_space->get_bounds()[3]->get_lower_bound();
q_min(4) = state_space->get_bounds()[4]->get_lower_bound();
q_min(5) = state_space->get_bounds()[5]->get_lower_bound();
q_min(6) = state_space->get_bounds()[6]->get_lower_bound();
q_min(7) = state_space->get_bounds()[7]->get_lower_bound();
q_min(8) = state_space->get_bounds()[15]->get_lower_bound();
q_max(0) = state_space->get_bounds()[0]->get_upper_bound();
q_max(1) = state_space->get_bounds()[1]->get_upper_bound();
q_max(2) = state_space->get_bounds()[2]->get_upper_bound();
q_max(3) = state_space->get_bounds()[3]->get_upper_bound();
q_max(4) = state_space->get_bounds()[4]->get_upper_bound();
q_max(5) = state_space->get_bounds()[5]->get_upper_bound();
q_max(6) = state_space->get_bounds()[6]->get_upper_bound();
q_max(7) = state_space->get_bounds()[7]->get_upper_bound();
q_max(8) = state_space->get_bounds()[15]->get_upper_bound();
KDL::ChainFkSolverPos_recursive fk_solver(chain1);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain1);
KDL::ChainIkSolverPos_NR_JL ik_solver(chain1,q_min,q_max,fk_solver,ik_solver_vel,1000,1e-6);
KDL::Frame F(KDL::Rotation::Quaternion(qx,qy,qz,qw),KDL::Vector(x,y,z));
bool ik_res = (ik_solver.CartToJnt(q,F,q_out)>=0);
if(ik_res)
{
std::vector<double> state_vec;
state_vec = state_var;
state_vec[0] = q_out(0);
state_vec[1] = q_out(1);
state_vec[2] = q_out(2);
state_vec[3] = q_out(3);
state_vec[4] = q_out(4);
state_vec[5] = q_out(5);
state_vec[6] = q_out(6);
state_vec[7] = q_out(7);
state_vec[15] = q_out(8);
state_vec[16] = set_grasping ? (double)GRIPPER_CLOSED : GRIPPER_OPEN;
state_space->copy_vector_to_point( state_vec, computed_state );
}
return ik_res;
}
static
bool Init(
KDLRobotModel* model,
const std::string& robot_description,
const std::string& base_link,
const std::string& tip_link,
int free_angle)
{
ROS_INFO("Initialize KDL Robot Model");
if (!model->m_urdf.initString(robot_description)) {
ROS_ERROR("Failed to parse the URDF.");
return false;
}
ROS_INFO("Initialize Robot Model");
urdf::JointSpec world_joint;
world_joint.name = "map";
world_joint.origin = Eigen::Affine3d::Identity(); // IMPORTANT
world_joint.axis = Eigen::Vector3d::Zero();
world_joint.type = urdf::JointType::Floating;
if (!urdf::InitRobotModel(
&model->m_robot_model, &model->m_urdf, &world_joint))
{
ROS_ERROR("Failed to initialize Robot Model");
return false;
}
ROS_INFO("Initialize KDL tree");
if (!kdl_parser::treeFromUrdfModel(model->m_urdf, model->m_tree)) {
ROS_ERROR("Failed to parse the kdl tree from robot description.");
return false;
}
ROS_INFO("Initialize KDL chain (%s, %s)", base_link.c_str(), tip_link.c_str());
if (!model->m_tree.getChain(base_link, tip_link, model->m_chain)) {
ROS_ERROR("Failed to fetch the KDL chain for the robot. (root: %s, tip: %s)", base_link.c_str(), tip_link.c_str());
return false;
}
model->m_base_link = base_link;
model->m_tip_link = tip_link;
model->m_kinematics_link = GetLink(&model->m_robot_model, &base_link);
if (model->m_kinematics_link == NULL) {
return false; // this shouldn't happen if the chain initialized successfully
}
ROS_INFO("Gather joints in chain");
std::vector<std::string> planning_joints;
for (auto i = (unsigned)0; i < model->m_chain.getNrOfSegments(); ++i) {
auto& segment = model->m_chain.getSegment(i);
auto& child_joint_name = segment.getJoint().getName();
auto* joint = GetJoint(&model->m_robot_model, &child_joint_name);
if (GetVariableCount(joint) > 1) {
ROS_WARN("> 1 variable per joint");
return false;
}
if (GetVariableCount(joint) == 0) {
continue;
}
planning_joints.push_back(child_joint_name);
}
ROS_INFO("Initialize URDF Robot Model with planning joints = %s", to_string(planning_joints).c_str());
if (!urdf::Init(model, &model->m_robot_model, &planning_joints)) {
ROS_ERROR("Failed to initialize URDF Robot Model");
return false;
}
// do this after we've initialized the URDFRobotModel...
model->planning_link = GetLink(&model->m_robot_model, &tip_link);
if (model->planning_link == NULL) {
return false; // this shouldn't happen either
}
// FK solver
model->m_fk_solver = make_unique<KDL::ChainFkSolverPos_recursive>(model->m_chain);
// IK Velocity solver
model->m_ik_vel_solver = make_unique<KDL::ChainIkSolverVel_pinv>(model->m_chain);
// IK solver
KDL::JntArray q_min(model->jointVariableCount());
KDL::JntArray q_max(model->jointVariableCount());
for (size_t i = 0; i < model->jointVariableCount(); ++i) {
if (model->vprops[i].continuous) {
q_min(i) = -M_PI;
q_max(i) = M_PI;
} else {
q_min(i) = model->vprops[i].min_position;
q_max(i) = model->vprops[i].max_position;
}
}
model->m_max_iterations = 200;
model->m_kdl_eps = 0.001;
model->m_ik_solver = make_unique<KDL::ChainIkSolverPos_NR_JL>(
model->m_chain,
q_min,
q_max,
*model->m_fk_solver,
*model->m_ik_vel_solver,
model->m_max_iterations,
model->m_kdl_eps);
model->m_jnt_pos_in.resize(model->m_chain.getNrOfJoints());
model->m_jnt_pos_out.resize(model->m_chain.getNrOfJoints());
model->m_free_angle = free_angle;
model->m_search_discretization = 0.02;
model->m_timeout = 0.005;
return true;
}
bool KDLRobotModel::init(std::string robot_description, std::vector<std::string> &planning_joints)
{
urdf_ = boost::shared_ptr<urdf::Model>(new urdf::Model());
if (!urdf_->initString(robot_description))
{
ROS_ERROR("Failed to parse the URDF.");
return false;
}
if (!kdl_parser::treeFromUrdfModel(*urdf_, ktree_))
{
ROS_ERROR("Failed to parse the kdl tree from robot description.");
return false;
}
std::vector<std::string> segments(planning_joints.size());
for(size_t j = 0; j < planning_joints.size(); ++j)
{
if(!leatherman::getSegmentOfJoint(ktree_, planning_joints[j], segments[j]))
{
ROS_ERROR("Failed to find kdl segment for '%s'.", planning_joints_[j].c_str());
return false;
}
}
/*
if(!leatherman::getChainTip(ktree_, segments, chain_root_name_, chain_tip_name_))
{
ROS_ERROR("Failed to find a valid chain tip link.");
return false;
}
*/
if(!ktree_.getChain(chain_root_name_, chain_tip_name_, kchain_))
{
ROS_ERROR("Failed to fetch the KDL chain for the robot. (root: %s, tip: %s)", chain_root_name_.c_str(), chain_tip_name_.c_str());
return false;
}
// check if our chain includes all planning joints
for(size_t i = 0; i < planning_joints.size(); ++i)
{
if(planning_joints[i].empty())
{
ROS_ERROR("Planning joint name is empty (index: %d).", int(i));
return false;
}
int index;
if(!leatherman::getJointIndex(kchain_, planning_joints[i], index))
{
ROS_ERROR("Failed to find '%s' in the kinematic chain. Maybe your chain root or tip joints are wrong? (%s, %s)", planning_joints[i].c_str(), chain_root_name_.c_str(), chain_tip_name_.c_str());
return false;
}
}
// joint limits
planning_joints_ = planning_joints;
if(!getJointLimits(planning_joints_, min_limits_, max_limits_, continuous_))
{
ROS_ERROR("Failed to get the joint limits.");
return false;
}
// FK solver
fk_solver_ = new KDL::ChainFkSolverPos_recursive(kchain_);
jnt_pos_in_.resize(kchain_.getNrOfJoints());
jnt_pos_out_.resize(kchain_.getNrOfJoints());
// IK solver
KDL::JntArray q_min(planning_joints_.size());
KDL::JntArray q_max(planning_joints_.size());
for(size_t i = 0; i < planning_joints_.size(); ++i)
{
q_min(i) = min_limits_[i];
q_max(i) = max_limits_[i];
}
ik_vel_solver_ = new KDL::ChainIkSolverVel_pinv(kchain_);
ik_solver_ = new KDL::ChainIkSolverPos_NR_JL(kchain_, q_min, q_max, *fk_solver_, *ik_vel_solver_, 200, 0.001);
// joint name -> index mapping
for(size_t i = 0; i < planning_joints_.size(); ++i)
joint_map_[planning_joints_[i]] = i;
// link name -> kdl index mapping
for(size_t i = 0; i < kchain_.getNrOfSegments(); ++i)
link_map_[kchain_.getSegment(i).getName()] = i;
initialized_ = true;
return true;
}
/*
=============
GLSLGamma_GammaCorrect
=============
*/
void GLSLGamma_GammaCorrect (void)
{
float smax, tmax;
if (!gl_glsl_gamma_able)
return;
if (vid_gamma.value == 1 && vid_contrast.value == 1)
return;
// create render-to-texture texture if needed
if (!r_gamma_texture)
{
glGenTextures (1, &r_gamma_texture);
glBindTexture (GL_TEXTURE_2D, r_gamma_texture);
r_gamma_texture_width = glwidth;
r_gamma_texture_height = glheight;
if (!gl_texture_NPOT)
{
r_gamma_texture_width = TexMgr_Pad(r_gamma_texture_width);
r_gamma_texture_height = TexMgr_Pad(r_gamma_texture_height);
}
glTexImage2D (GL_TEXTURE_2D, 0, GL_RGBA8, r_gamma_texture_width, r_gamma_texture_height, 0, GL_BGRA, GL_UNSIGNED_INT_8_8_8_8_REV, NULL);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
}
// create shader if needed
if (!r_gamma_program)
{
GLSLGamma_CreateShaders ();
if (!r_gamma_program)
{
Sys_Error("GLSLGamma_CreateShaders failed");
}
}
// copy the framebuffer to the texture
GL_DisableMultitexture();
glBindTexture (GL_TEXTURE_2D, r_gamma_texture);
glCopyTexSubImage2D (GL_TEXTURE_2D, 0, 0, 0, glx, gly, glwidth, glheight);
// draw the texture back to the framebuffer with a fragment shader
GL_UseProgramFunc (r_gamma_program);
GL_Uniform1fFunc (gammaLoc, vid_gamma.value);
GL_Uniform1fFunc (contrastLoc, q_min(2.0, q_max(1.0, vid_contrast.value)));
GL_Uniform1iFunc (textureLoc, 0); // use texture unit 0
glDisable (GL_ALPHA_TEST);
glDisable (GL_DEPTH_TEST);
glViewport (glx, gly, glwidth, glheight);
smax = glwidth/(float)r_gamma_texture_width;
tmax = glheight/(float)r_gamma_texture_height;
glBegin (GL_QUADS);
glTexCoord2f (0, 0);
glVertex2f (-1, -1);
glTexCoord2f (smax, 0);
glVertex2f (1, -1);
glTexCoord2f (smax, tmax);
glVertex2f (1, 1);
glTexCoord2f (0, tmax);
glVertex2f (-1, 1);
glEnd ();
GL_UseProgramFunc (0);
// clear cached binding
GL_ClearBindings ();
}
/*
==================
V_CalcRefdef
==================
*/
void V_CalcRefdef (void)
{
entity_t *ent, *view;
int i;
vec3_t forward, right, up;
vec3_t angles;
float bob;
static float oldz = 0;
static vec3_t punch = {0,0,0}; //johnfitz -- v_gunkick
float delta; //johnfitz -- v_gunkick
V_DriftPitch ();
// ent is the player model (visible when out of body)
ent = &cl_entities[cl.viewentity];
// view is the weapon model (only visible from inside body)
view = &cl.viewent;
// transform the view offset by the model's matrix to get the offset from
// model origin for the view
ent->angles[YAW] = cl.viewangles[YAW]; // the model should face the view dir
ent->angles[PITCH] = -cl.viewangles[PITCH]; // the model should face the view dir
bob = V_CalcBob ();
// refresh position
VectorCopy (ent->origin, r_refdef.vieworg);
r_refdef.vieworg[2] += cl.viewheight + bob;
// never let it sit exactly on a node line, because a water plane can
// dissapear when viewed with the eye exactly on it.
// the server protocol only specifies to 1/16 pixel, so add 1/32 in each axis
r_refdef.vieworg[0] += 1.0/32;
r_refdef.vieworg[1] += 1.0/32;
r_refdef.vieworg[2] += 1.0/32;
VectorCopy (cl.viewangles, r_refdef.viewangles);
V_CalcViewRoll ();
V_AddIdle ();
// offsets
angles[PITCH] = -ent->angles[PITCH]; // because entity pitches are actually backward
angles[YAW] = ent->angles[YAW];
angles[ROLL] = ent->angles[ROLL];
AngleVectors (angles, forward, right, up);
if (cl.maxclients <= 1) //johnfitz -- moved cheat-protection here from V_RenderView
for (i=0 ; i<3 ; i++)
r_refdef.vieworg[i] += scr_ofsx.value*forward[i] + scr_ofsy.value*right[i] + scr_ofsz.value*up[i];
V_BoundOffsets ();
// set up gun position
VectorCopy (cl.viewangles, view->angles);
CalcGunAngle ();
VectorCopy (ent->origin, view->origin);
view->origin[2] += cl.viewheight;
for (i=0 ; i<3 ; i++)
view->origin[i] += forward[i]*bob*0.4;
view->origin[2] += bob;
//johnfitz -- removed all gun position fudging code (was used to keep gun from getting covered by sbar)
//MarkV -- restored this with r_viewmodel_quake cvar
if (r_viewmodel_quake.value)
{
if (scr_viewsize.value == 110)
view->origin[2] += 1;
else if (scr_viewsize.value == 100)
view->origin[2] += 2;
else if (scr_viewsize.value == 90)
view->origin[2] += 1;
else if (scr_viewsize.value == 80)
view->origin[2] += 0.5;
}
view->model = cl.model_precache[cl.stats[STAT_WEAPON]];
view->frame = cl.stats[STAT_WEAPONFRAME];
view->colormap = vid.colormap;
//johnfitz -- v_gunkick
if (v_gunkick.value == 1) //original quake kick
VectorAdd (r_refdef.viewangles, cl.punchangle, r_refdef.viewangles);
if (v_gunkick.value == 2) //lerped kick
{
for (i=0; i<3; i++)
if (punch[i] != v_punchangles[0][i])
{
//speed determined by how far we need to lerp in 1/10th of a second
delta = (v_punchangles[0][i]-v_punchangles[1][i]) * host_frametime * 10;
if (delta > 0)
punch[i] = q_min(punch[i]+delta, v_punchangles[0][i]);
else if (delta < 0)
punch[i] = q_max(punch[i]+delta, v_punchangles[0][i]);
}
VectorAdd (r_refdef.viewangles, punch, r_refdef.viewangles);
}
//johnfitz
// smooth out stair step ups
if (!noclip_anglehack && cl.onground && ent->origin[2] - oldz > 0) //johnfitz -- added exception for noclip
//FIXME: noclip_anglehack is set on the server, so in a nonlocal game this won't work.
{
float steptime;
steptime = cl.time - cl.oldtime;
if (steptime < 0)
//FIXME I_Error ("steptime < 0");
steptime = 0;
oldz += steptime * 80;
if (oldz > ent->origin[2])
oldz = ent->origin[2];
if (ent->origin[2] - oldz > 12)
oldz = ent->origin[2] - 12;
r_refdef.vieworg[2] += oldz - ent->origin[2];
view->origin[2] += oldz - ent->origin[2];
}
else
oldz = ent->origin[2];
if (chase_active.value)
Chase_UpdateForDrawing (); //johnfitz
}
