void PoseSeqItem::onPositionChanged()
{
if(!sigInterpolationParametersChangedConnection.connected()){
sigInterpolationParametersChangedConnection =
generationBar->sigInterpolationParametersChanged().connect(
std::bind(&PoseSeqItem::updateInterpolationParameters, this));
updateInterpolationParameters();
}
BodyItemPtr prevBodyItem = ownerBodyItem;
ownerBodyItem = findOwnerItem<BodyItem>();
if(ownerBodyItem == prevBodyItem){
return;
}
if(!ownerBodyItem){
interpolator_->setBody(0);
} else {
Body* body = ownerBodyItem->body();
if(seq->targetBodyName().empty()){
seq->setTargetBodyName(body->name());
} else if(prevBodyItem && (seq->targetBodyName() != body->name())){
convert(prevBodyItem->body());
}
interpolator_->setBody(body);
const Listing& linearInterpolationJoints =
*ownerBodyItem->body()->info()->findListing("linearInterpolationJoints");
if(linearInterpolationJoints.isValid()){
for(int i=0; i < linearInterpolationJoints.size(); ++i){
Link* link = body->link(linearInterpolationJoints[i].toString());
if(link){
interpolator_->setLinearInterpolationJoint(link->jointId());
}
}
}
LeggedBodyHelperPtr legged = getLeggedBodyHelper(ownerBodyItem->body());
if(legged->isValid()){
for(int i=0; i < legged->numFeet(); ++i){
interpolator_->addFootLink(legged->footLink(i)->index(), legged->centerOfSoleLocal(i));
}
}
interpolator_->setLipSyncShapes(*ownerBodyItem->body()->info()->findMapping("lipSyncShapes"));
bodyMotionItem_->motion()->setNumJoints(interpolator_->body()->numJoints());
if(generationBar->isAutoGenerationForNewBodyEnabled()){
updateTrajectory(true);
} else {
bodyMotionItem_->notifyUpdate();
}
}
}
bool WorldRosItem::spawnModel(gazebo_msgs::SpawnModel::Request &req,
gazebo_msgs::SpawnModel::Response &res)
{
std::string model_name = req.model_name;
std::string model_xml = req.model_xml;
cnoid::Vector3 trans;
cnoid::Quat R;
trans(0) = req.initial_pose.position.x;
trans(1) = req.initial_pose.position.y;
trans(2) = req.initial_pose.position.z;
R.w() = req.initial_pose.orientation.w;
R.x() = req.initial_pose.orientation.x;
R.y() = req.initial_pose.orientation.y;
R.z() = req.initial_pose.orientation.z;
BodyItemPtr body = new BodyItem();
body->setName(req.model_name);
const char *fname = tmpnam(NULL);
std::ofstream ofs(fname);
ofs << model_xml << std::endl;
ofs.close();
body->loadModelFile(fname);
remove(fname);
body->body()->rootLink()->setTranslation(trans);
body->body()->rootLink()->setRotation(R.matrix());
world->addChildItem(body);
BodyRosItemPtr bodyros = new BodyRosItem();
bodyros->setName(req.model_name + "_ROS");
body->addChildItem(bodyros);
ItemTreeView::instance()->checkItem(body);
ItemTreeView::instance()->checkItem(bodyros);
return true;
}
static bool bodyMotionItemPreFilter(BodyMotionItem* protoItem, Item* parentItem)
{
BodyItemPtr bodyItem = dynamic_cast<BodyItem*>(parentItem);
if(!bodyItem){
bodyItem = parentItem->findOwnerItem<BodyItem>();
}
if(bodyItem){
int prevNumJoints = protoItem->jointPosSeq()->numParts();
int numJoints = bodyItem->body()->numJoints();
if(numJoints != prevNumJoints){
protoItem->jointPosSeq()->setNumParts(numJoints, true);
}
}
return true;
}
EditableSceneBody::EditableSceneBody(BodyItemPtr bodyItem)
: SceneBody(bodyItem->body(), createEditableSceneLink)
{
setName(body()->name());
impl = new EditableSceneBodyImpl(this, bodyItem);
}
bool BodyUnit::initialize(BodyItemPtr bodyItem)
{
if(TRACE_FUNCTIONS){
cout << "BodyUnit::initialize()" << endl;
}
if(bodyItem->body()->isStaticModel()){
return false;
}
frame = 0;
body = bodyItem->body()->duplicate();
baseLink = 0;
ItemList<BodyMotionItem> motionItems = bodyItem->getSubItems<BodyMotionItem>();
if(!motionItems.empty()){
BodyMotionItemPtr motionItem;
// prefer the first checked item
for(size_t i=0; i < motionItems.size(); ++i){
if(ItemTreeView::mainInstance()->isItemChecked(motionItems[i])){
motionItem = motionItems[i];
break;
}
}
if(!motionItem){
motionItem = motionItems[0];
}
frameRate = motionItem->motion()->frameRate();
qseqRef = motionItem->jointPosSeq();
if(qseqResult){
qseqResultBuf.reset(new MultiValueSeq(qseqResult->numParts(), 0, frameRate));
}
if(motionItem->hasRelativeZmpSeqItem()){
zmpSeq = motionItem->relativeZmpSeq();
}
rootResultItem = motionItem->linkPosSeqItem();
rootResultBuf.reset(new MultiAffine3Seq(1, 0, frameRate));
rootResult = motionItem->linkPosSeq();
rootResult->setFrameRate(frameRate);
rootResult->setDimension(1, 1);
}
const YamlMapping& info = *bodyItem->body()->info();
const YamlSequence& footLinkInfos = *info.findSequence("footLinks");
if(footLinkInfos.isValid()){
for(int i=0; i < footLinkInfos.size(); ++i){
const YamlMapping& footLinkInfo = *footLinkInfos[i].toMapping();
Link* link = body->link(footLinkInfo["link"].toString());
Vector3 soleCenter;
if(link && read(footLinkInfo, "soleCenter", soleCenter)){
footLinks.push_back(link);
footLinkOriginHeights.push_back(-soleCenter[2]);
}
}
}
if(!footLinks.empty()){
if(zmpSeq){
setBaseLinkByZmp(0);
} else {
if(bodyItem->currentBaseLink()){
int currentBaseLinkIndex = bodyItem->currentBaseLink()->index;
for(size_t i=0; i < footLinks.size(); ++i){
Link* footLink = footLinks[i];
if(footLink->index == currentBaseLinkIndex){
setBaseLink(footLink, footLinkOriginHeights[i]);
}
}
} else{
setBaseLink(footLinks[0], footLinkOriginHeights[0]);
}
}
}
if(!baseLink){
if(bodyItem->currentBaseLink()){
baseLink = body->link(bodyItem->currentBaseLink()->index);
} else {
baseLink = body->rootLink();
}
fkTraverse.find(baseLink, true, true);
}
if(rootResult){
Link* rootLink = body->rootLink();
Affine3& initialPos = rootResult->at(0, 0);
initialPos.translation() = rootLink->p;
initialPos.linear() = rootLink->R;
}
return (qseqRef != 0);
}
bool isEditable() {
return bodyItem->isEditable() &&
(!bodyItem->body()->isStaticModel() || enableStaticModelEditCheck->isChecked());
}