bool ComponentHealth::rayCast(OBJECT_ID actor,
const vec3 &p,
const vec3 &d,
float l)
{
ASSERT(world, "Member \"world\" world was null");
return world->getPhysicsEngine()->rayCast(getGeom(actor),
getGeom(*getActorPtr()),
p, d, l);
}
void moveit_rviz_plugin::PerLinkSubObjBase::changed()
{
shapes_.reset();
centers_.clear();
radii_.clear();
if (!getBool())
return;
getGeom(robot_relative_, centers_, radii_);
if (centers_.empty())
return;
shapes_.reset(new ShapesDisplay(getSceneNode(), base_->getColor(), base_->getSize()));
if (centers_.size() == radii_.size() || radii_.size() == 1)
{
shapes_->addSpheres(centers_, radii_);
base_->setStyle(PerLinkObjBase::SPHERES);
}
else if (base_->getStyle() == PerLinkObjBase::SPHERES)
{
shapes_->addSpheres(centers_, base_->getSize() * 0.5);
}
else
{
shapes_->addPoints(centers_);
}
}
// called by Webots at the beginning of the simulation
void webots_physics_init(dWorldID w, dSpaceID s, dJointGroupID j) {
int i;
// store global objects for later use
world = w;
space = s;
contact_joint_group = j;
// get floor geometry
floor_geom = getGeom(floor_name);
if (!floor_geom)
return;
// get foot geometry and body id's
for (i = 0; i < N_FEET; i++) {
foot_geom[i] = getGeom(foot_name[i]);
if (!foot_geom[i])
return;
foot_body[i] = dGeomGetBody(foot_geom[i]);
if (!foot_body[i])
return;
}
// create universal joints for linear actuators
for (i = 0; i < 10; i++) {
dBodyID upper_piston = getBody(upper_piston_name[i]);
dBodyID lower_piston = getBody(lower_piston_name[i]);
dBodyID upper_link = getBody(upper_link_name[i]);
dBodyID lower_link = getBody(lower_link_name[i]);
if (!upper_piston || !lower_piston || !upper_link || !lower_link)
return;
// create a ball and socket joint (3 DOFs) to attach the lower piston body to the lower link
// we don't need a universal joint here, because the piston's passive rotation is prevented
// by the universal joint at its upper end.
dJointID lower_balljoint = dJointCreateBall(world, 0);
dJointAttach(lower_balljoint, lower_piston, lower_link);
// transform attachement point from local to global coordinate system
// warning: this is a hard-coded translation
dVector3 lower_ball;
dBodyGetRelPointPos(lower_piston, 0, 0, -0.075, lower_ball);
// set attachement point (anchor)
dJointSetBallAnchor(lower_balljoint, lower_ball[0], lower_ball[1], lower_ball[2]);
// create a universal joint (2 DOFs) to attach upper piston body to upper link
// we need to use a universal joint to prevent the piston from passively rotating around its long axis
dJointID upper_ujoint = dJointCreateUniversal(world, 0);
dJointAttach(upper_ujoint, upper_piston, upper_link);
// transform attachement point from local to global coordinate system
// warning: this is a hard-coded translation
dVector3 upper_ball;
dBodyGetRelPointPos(upper_piston, 0, 0, 0, upper_ball);
// set attachement point (anchor)
dJointSetUniversalAnchor(upper_ujoint, upper_ball[0], upper_ball[1], upper_ball[2]);
// set the universal joint axes
dVector3 upper_xaxis;
dVector3 upper_yaxis;
dBodyVectorToWorld(upper_piston, 1, 0, 0, upper_xaxis);
dBodyVectorToWorld(upper_piston, 0, 1, 0, upper_yaxis);
dJointSetUniversalAxis1(upper_ujoint, upper_xaxis[0], upper_xaxis[1], upper_xaxis[2]);
dJointSetUniversalAxis2(upper_ujoint, upper_yaxis[0], upper_yaxis[1], upper_yaxis[2]);
}
}
dGeomID ComponentHealth::getGeom(OBJECT_ID actor)
{
ASSERT(world, "Member \"world\" world was null");
return getGeom(world->getObjects().get(actor));
}
