void WorldItemImpl::updateCollisions(bool forceUpdate)
{
for(BodyItemInfoMap::iterator p = bodyItemInfoMap.begin(); p != bodyItemInfoMap.end(); ++p){
BodyItemInfo& info = p->second;
BodyItem* bodyItem = p->first;
bodyItem->clearCollisions();
if(info.kinematicStateChanged || forceUpdate){
const BodyPtr& body = bodyItem->body();
for(int i=0; i < body->numLinks(); ++i){
collisionDetector->updatePosition(info.geometryId + i, body->link(i)->T());
}
}
info.kinematicStateChanged = false;
}
collisions->clear();
collisionDetector->detectCollisions(std::bind(&WorldItemImpl::extractCollisions, this, _1));
sceneCollision->setDirty();
for(BodyItemInfoMap::iterator p = bodyItemInfoMap.begin(); p != bodyItemInfoMap.end(); ++p){
BodyItem* bodyItem = p->first;
BodyItemInfo& info = p->second;
bodyItem->notifyCollisionUpdate();
}
sigCollisionsUpdated();
}
void WorldRosItem::publishModelStates()
{
gazebo_msgs::ModelStates model_states;
Item* item = world->childItem();
while(item) {
BodyItem* body = boost::dynamic_pointer_cast<BodyItem>(item);
if (body) {
model_states.name.push_back(body->name());
Link* link = body->body()->rootLink();
Vector3 pos = link->translation();
Quat rot = Quat(link->rotation());
geometry_msgs::Pose pose;
pose.position.x = pos(0);
pose.position.y = pos(1);
pose.position.z = pos(2);
pose.orientation.w = rot.w();
pose.orientation.x = rot.x();
pose.orientation.y = rot.y();
pose.orientation.z = rot.z();
model_states.pose.push_back(pose);
/*
twist.linear.x = linear_vel.x;
twist.linear.y = linear_vel.y;
twist.linear.z = linear_vel.z;
twist.angular.x = angular_vel.x;
twist.angular.y = angular_vel.y;
twist.angular.z = angular_vel.z;
model_states.twist.push_back(twist);
*/
}
item = item->nextItem();
}
pub_model_states_.publish(model_states);
}
bool EditableSceneBodyImpl::storeProperties(Archive& archive)
{
ListingPtr states = new Listing();
ItemList<BodyItem> bodyItems;
bodyItems.extractChildItems(RootItem::instance());
for(size_t i=0; i < bodyItems.size(); ++i){
BodyItem* bodyItem = bodyItems[i];
EditableSceneBody* sceneBody = bodyItem->existingSceneBody();
if(sceneBody){
ValueNodePtr id = archive.getItemId(bodyItem);
if(id){
EditableSceneBodyImpl* impl = sceneBody->impl;
MappingPtr state = new Mapping();
state->insert("bodyItem", id);
state->write("showCenterOfMass", impl->isCmVisible);
state->write("showZmp", impl->isZmpVisible);
states->append(state);
}
}
}
if(!states->empty()){
archive.insert("editableSceneBodies", states);
return true;
}
return false;
}
void KinematicFaultCheckerImpl::apply()
{
bool processed = false;
ItemList<BodyMotionItem> items = ItemTreeView::mainInstance()->selectedItems<BodyMotionItem>();
if(items.empty()){
mes.notify(_("No BodyMotionItems are selected."));
} else {
for(size_t i=0; i < items.size(); ++i){
BodyMotionItem* motionItem = items.get(i);
BodyItem* bodyItem = motionItem->findOwnerItem<BodyItem>();
if(!bodyItem){
mes.notify(str(fmt(_("%1% is not owned by any BodyItem. Check skiped.")) % motionItem->name()));
} else {
mes.putln();
mes.notify(str(fmt(_("Applying the Kinematic Fault Checker to %1% ..."))
% motionItem->headItem()->name()));
dynamic_bitset<> linkSelection;
if(selectedJointsRadio.isChecked()){
linkSelection = LinkSelectionView::mainInstance()->linkSelection(bodyItem);
} else if(nonSelectedJointsRadio.isChecked()){
linkSelection = LinkSelectionView::mainInstance()->linkSelection(bodyItem);
linkSelection.flip();
} else {
linkSelection.resize(bodyItem->body()->numLinks(), true);
}
double beginningTime = 0.0;
double endingTime = motionItem->motion()->getTimeLength();
std::numeric_limits<double>::max();
if(onlyTimeBarRangeCheck.isChecked()){
TimeBar* timeBar = TimeBar::instance();
beginningTime = timeBar->minTime();
endingTime = timeBar->maxTime();
}
int n = checkFaults(bodyItem, motionItem, mes.cout(),
positionCheck.isChecked(),
velocityCheck.isChecked(),
collisionCheck.isChecked(),
linkSelection,
beginningTime, endingTime);
if(n > 0){
if(n == 1){
mes.notify(_("A fault has been detected."));
} else {
mes.notify(str(fmt(_("%1% faults have been detected.")) % n));
}
} else {
mes.notify(_("No faults have been detected."));
}
processed = true;
}
}
}
}
/**
\todo reduce the number of calling this function when the project is loaded.
*/
void WorldItemImpl::updateCollisionDetector(bool forceUpdate)
{
if(TRACE_FUNCTIONS){
os << "WorldItemImpl::updateCollisionDetector()" << endl;
}
if(!isCollisionDetectionEnabled){
return;
}
if(forceUpdate){
updateColdetBodyInfos(coldetBodyInfos);
} else {
vector<ColdetBodyInfo> infos;
updateColdetBodyInfos(infos);
if(infos.size() == coldetBodyInfos.size()){
if(std::equal(infos.begin(), infos.end(), coldetBodyInfos.begin(),
[](ColdetBodyInfo& info1, ColdetBodyInfo& info2){
return (info1.bodyItem == info2.bodyItem &&
info1.isSelfCollisionDetectionEnabled == info2.isSelfCollisionDetectionEnabled); })){
return; // not changed
}
}
coldetBodyInfos = infos;
}
clearCollisionDetector();
for(auto& bodyInfo : coldetBodyInfos){
BodyItem* bodyItem = bodyInfo.bodyItem;
bodyCollisionDetector.addBody(
bodyItem->body(), bodyInfo.isSelfCollisionDetectionEnabled,
[&bodyInfo](Link* link, GeometryHandle geometry){
ColdetLinkInfo* linkInfo = new ColdetLinkInfo(bodyInfo.bodyItem, link, geometry);
bodyInfo.linkInfos.push_back(linkInfo);
return linkInfo;
});
ColdetBodyInfo* pBodyInfo = &bodyInfo;
sigKinematicStateChangedConnections.add(
bodyItem->sigKinematicStateChanged().connect(
[&, pBodyInfo](){
pBodyInfo->kinematicStateChanged = true;
updateCollisionsLater.setPriority(kinematicsBar->collisionDetectionPriority());
updateCollisionsLater();
}));
}
bodyCollisionDetector.makeReady();
updateCollisions(true);
}
void EditableSceneBodyImpl::restoreProperties(const Archive& archive)
{
Listing& states = *archive["editableSceneBodies"].toListing();
for(int i=0; i < states.size(); ++i){
Mapping* state = states[i].toMapping();
BodyItem* bodyItem = archive.findItem<BodyItem>(state->find("bodyItem"));
if(bodyItem){
EditableSceneBodyImpl* impl = bodyItem->sceneBody()->impl;
impl->showCenterOfMass(state->get("showCenterOfMass", impl->isCmVisible));
impl->showZmp(state->get("showZmp", impl->isZmpVisible));
}
}
}
void BodyItemImpl::createPenetrationBlocker(Link* link, bool excludeSelfCollisions, PenetrationBlockerPtr& blocker)
{
WorldItem* worldItem = self->findOwnerItem<WorldItem>();
if(worldItem){
blocker = boost::make_shared<PenetrationBlocker>(worldItem->collisionDetector()->clone(), link);
const ItemList<BodyItem>& bodyItems = worldItem->bodyItems();
for(int i=0; i < bodyItems.size(); ++i){
BodyItem* bodyItem = bodyItems.get(i);
if(bodyItem != self && bodyItem->body()->isStaticModel()){
blocker->addOpponentLink(bodyItem->body()->rootLink());
}
}
blocker->setDepth(kinematicsBar->penetrationBlockDepth());
blocker->start();
}
}
void LinkPropertyViewImpl::updateProperties()
{
clear();
BodyItem* bodyItem = linkSelectionView->currentBodyItem();
if(bodyItem){
Body* body = bodyItem->body();
int linkIndex = linkSelectionView->selectedLinkIndex();
if(linkIndex >= 0){
Link* link = body->link(linkIndex);
if(link){
updateLinkProperties(link);
}
}
}
}
/**
\todo reduce the number of calling this function when the project is loaded.
*/
void WorldItemImpl::updateCollisionDetector(bool forceUpdate)
{
if(TRACE_FUNCTIONS){
os << "WorldItemImpl::updateCollisionDetector()" << endl;
}
if(!isCollisionDetectionEnabled){
return;
}
if(!forceUpdate){
ItemList<BodyItem> prevBodyItems = collisionBodyItems;
boost::dynamic_bitset<> prevSelfCollisionFlags = collisionBodyItemsSelfCollisionFlags;
updateCollisionBodyItems();
if(collisionBodyItems == prevBodyItems &&
collisionBodyItemsSelfCollisionFlags == prevSelfCollisionFlags){
return;
}
} else {
updateCollisionBodyItems();
}
clearCollisionDetector();
for(size_t i=0; i < collisionBodyItems.size(); ++i){
BodyItem* bodyItem = collisionBodyItems.get(i);
const BodyPtr& body = bodyItem->body();
const int numLinks = body->numLinks();
pair<BodyItemInfoMap::iterator, bool> inserted =
bodyItemInfoMap.insert(make_pair(bodyItem, BodyItemInfo()));
BodyItemInfo& info = inserted.first->second;
info.geometryId = addBodyToCollisionDetector(
*body, *collisionDetector, bodyItem->isSelfCollisionDetectionEnabled());
geometryIdToBodyInfoMap.resize(collisionDetector->numGeometries(), inserted.first);
sigKinematicStateChangedConnections.add(
bodyItem->sigKinematicStateChanged().connect(
std::bind(&WorldItemImpl::onBodyKinematicStateChanged, this, bodyItem)));
}
collisionDetector->makeReady();
updateCollisions(true);
}
BodyInfo(BodyItem* bodyItem){
this->bodyItem = bodyItem;
if(bodyItem){
body = bodyItem->body();
deviceInfos.resize(body->numDevices());
} else {
body = 0;
}
}
void LeggedBodyBarImpl::moveCM(BodyItem::PositionType position)
{
const ItemList<BodyItem>& targetBodyItems = bodyBar->targetBodyItems();
for(size_t i=0; i < targetBodyItems.size(); ++i){
BodyItem* bodyItem = targetBodyItems[i];
Vector3 c = bodyItem->centerOfMass();
boost::optional<Vector3> p = bodyItem->getParticularPosition(position);
if(p){
c[0] = (*p)[0];
c[1] = (*p)[1];
}
if(!bodyItem->doLegIkToMoveCm(c, true)){
MessageView::instance()->notify(
fmt::format(_("The center of mass of {} cannt be moved to the target position\n"),
bodyItem->name()));
}
}
}
void WorldItemImpl::extractCollisions(const CollisionPair& collisionPair)
{
CollisionLinkPairPtr collisionLinkPair = std::make_shared<CollisionLinkPair>();
collisionLinkPair->collisions = collisionPair.collisions();
BodyItem* bodyItem = 0;
for(int i=0; i < 2; ++i){
ColdetLinkInfo* linkInfo = static_cast<ColdetLinkInfo*>(collisionPair.object(i));
if(linkInfo->bodyItem != bodyItem){
bodyItem = linkInfo->bodyItem;
bodyItem->collisions().push_back(collisionLinkPair);
}
collisionLinkPair->body[i] = bodyItem->body();
Link* link = linkInfo->link;
collisionLinkPair->link[i] = link;
bodyItem->collisionsOfLink(link->index()).push_back(collisionLinkPair);
bodyItem->collisionLinkBitSet().set(link->index());
}
collisions->push_back(collisionLinkPair);
}
void BodyMotionGenerationBar::onGenerationButtonClicked()
{
set<BodyMotionItem*> motionItems; // for avoiding overlap
ItemList<Item> selectedItems = ItemTreeView::mainInstance()->selectedItems<Item>();
for(size_t i=0; i < selectedItems.size(); ++i){
PoseSeqItem* poseSeqItem = dynamic_cast<PoseSeqItem*>(selectedItems[i]);
if(poseSeqItem){
motionItems.insert(poseSeqItem->bodyMotionItem());
} else {
BodyMotionItem* motionItem = dynamic_cast<BodyMotionItem*>(selectedItems[i]);
if(motionItem){
motionItems.insert(motionItem);
}
}
}
for(set<BodyMotionItem*>::iterator p = motionItems.begin(); p != motionItems.end(); ++p){
BodyMotionItem* motionItem = *p;
BodyItem* bodyItem = motionItem->findOwnerItem<BodyItem>(true);
if(bodyItem){
PoseProvider* provider = 0;
PoseSeqItem* poseSeqItem = dynamic_cast<PoseSeqItem*>(motionItem->parentItem());
if(poseSeqItem){
provider = poseSeqItem->interpolator().get();
} else {
bodyMotionPoseProvider->initialize(bodyItem->body(), motionItem->motion());
provider = bodyMotionPoseProvider;
if(setup->newBodyItemCheck.isChecked()){
BodyMotionItem* newMotionItem = new BodyMotionItem();
newMotionItem->setName(motionItem->name() + "'");
motionItem->parentItem()->insertChildItem(newMotionItem, motionItem->nextItem());
motionItem = newMotionItem;
}
}
shapeBodyMotion(bodyItem->body(), provider, motionItem, true);
}
}
}
void WorldItemImpl::extractCollisions(const CollisionPair& collisionPair)
{
CollisionLinkPairPtr collisionLinkPair = std::make_shared<CollisionLinkPair>();
collisionLinkPair->collisions = collisionPair.collisions;
BodyItem* bodyItem = 0;
for(int i=0; i < 2; ++i){
const int geometryId = collisionPair.geometryId[i];
BodyItemInfoMap::const_iterator p = geometryIdToBodyInfoMap[geometryId];
const BodyItemInfo& info = p->second;
const int linkIndex = geometryId - info.geometryId;
if(p->first != bodyItem){
bodyItem = p->first;
bodyItem->collisions().push_back(collisionLinkPair);
}
const BodyPtr& body = bodyItem->body();
collisionLinkPair->body[i] = body;
collisionLinkPair->link[i] = body->link(linkIndex);
bodyItem->collisionsOfLink(linkIndex).push_back(collisionLinkPair);
bodyItem->collisionLinkBitSet().set(linkIndex);
}
collisions->push_back(collisionLinkPair);
}
void BodyItemImpl::doAssign(Item* srcItem)
{
BodyItem* srcBodyItem = dynamic_cast<BodyItem*>(srcItem);
if(srcBodyItem){
// copy the base link property
Link* baseLink = 0;
Link* srcBaseLink = srcBodyItem->currentBaseLink();
if(srcBaseLink){
baseLink = body->link(srcBaseLink->name());
if(baseLink){
setCurrentBaseLink(baseLink);
}
}
// copy the current kinematic state
Body* srcBody = srcBodyItem->body();
for(int i=0; i < srcBody->numLinks(); ++i){
Link* srcLink = srcBody->link(i);
Link* link = body->link(srcLink->name());
if(link){
link->q() = srcLink->q();
}
}
if(baseLink){
baseLink->p() = srcBaseLink->p();
baseLink->R() = srcBaseLink->R();
} else {
body->rootLink()->p() = srcBody->rootLink()->p();
body->rootLink()->R() = srcBody->rootLink()->R();
}
zmp = srcBodyItem->impl->zmp;
initialState = srcBodyItem->impl->initialState;
self->notifyKinematicStateChange(true);
}
}
