void PoseSeqItem::convert(BodyPtr orgBody)
{
if(!orgBody){
return;
}
const Listing& convInfoTop = *ownerBodyItem->body()->info()->findListing("poseConversionInfo");
if(convInfoTop.isValid()){
for(int i=0; i < convInfoTop.size(); ++i){
const Mapping& convInfo = *convInfoTop[i].toMapping();
const Listing& targets = *convInfo["targetBodies"].toListing();
for(int j=0; j < targets.size(); ++j){
if(targets[j].toString() == orgBody->name()){
beginEditing();
if(endEditing(convertSub(orgBody, convInfo))){
clearFileInformation();
BodyPtr body = ownerBodyItem->body();
seq->setTargetBodyName(body->name());
format m(_("Pose seq \"%1%\" has been converted. Its target has been changed from %2% to %3%"));
MessageView::mainInstance()->notify(str(m % name() % orgBody->name() % body->name()));
return;
}
}
}
}
}
}
void ButtonComponent::onMouseEvent(const MouseEvent& mouseEvent, SystemManagerPtr systemManager) {
PhysicsSystemPtr physicsSystem = makeShared(systemManager->getSystemByType<PhysicsSystem>(SystemType::PHYSICS));
GraphicsSystemPtr graphicsSystem = makeShared(systemManager->getSystemByType<GraphicsSystem>(SystemType::GRAPHICS));
if (mouseEvent.action == MouseAction::MOVE) {
BodyPtr body = makeShared(physicsSystem->getBody(entityId));
if (body->checkPoint(*mouseEvent.position)) {
ButtonDrawablePtr drawable = static_pointer_cast<ButtonDrawable>(makeShared(graphicsSystem->getDrawableById(entityId)));
drawable->state = ButtonState::OVER;
}
else {
ButtonDrawablePtr drawable = static_pointer_cast<ButtonDrawable>(makeShared(graphicsSystem->getDrawableById(entityId)));
drawable->state = ButtonState::UP;
}
}
else if (mouseEvent.action == MouseAction::CLICK_DOWN) {
ButtonDrawablePtr drawable = static_pointer_cast<ButtonDrawable>(makeShared(graphicsSystem->getDrawableById(entityId)));
if (drawable->state == ButtonState::OVER) {
drawable->state = ButtonState::DOWN;
}
}
else {
ButtonDrawablePtr drawable = static_pointer_cast<ButtonDrawable>(makeShared(graphicsSystem->getDrawableById(entityId)));
if (drawable->state == ButtonState::DOWN) {
drawable->state = ButtonState::OVER;
mCallback();
}
}
}
void JointSliderViewImpl::updateJointPositions()
{
BodyPtr body = currentBodyItem->body();
for(size_t i=0; i < activeJointIds.size(); ++i){
int jointId = activeJointIds[i];
jointSliders[i]->updatePosition(body->joint(jointId));
}
}
void JointSliderViewImpl::onUnitChanged()
{
BodyPtr body = currentBodyItem->body();
for(size_t i=0; i < activeJointIds.size(); ++i){
int jointId = activeJointIds[i];
jointSliders[jointId]->initialize(body->joint(jointId));
}
}
virtual bool initialize() {
if(!qseq){
string filename = getNativePathString(
boost::filesystem::path(shareDirectory())
/ "motion" / "SR1" / "SR1WalkPattern3.yaml");
BodyMotion motion;
if(!motion.loadStandardYAMLformat(filename)){
os() << motion.seqMessage() << endl;
return false;
}
qseq = motion.jointPosSeq();
if(qseq->numFrames() == 0){
os() << "Empty motion data." << endl;
return false;
}
timeStep_ = qseq->getTimeStep();
}
if(fabs(timeStep() - timeStep_) > 1.0e-6){
os() << "Time step must be " << timeStep_ << "." << endl;;
return false;
}
body = ioBody();
numJoints = body->numJoints();
if(numJoints != qseq->numParts()){
os() << "The number of joints must be " << qseq->numParts() << endl;
return false;
}
qold.resize(numJoints);
VectorXd q0(numJoints);
VectorXd q1(numJoints);
for(int i=0; i < numJoints; ++i){
qold[i] = q0[i] = body->joint(i)->q();
}
MultiValueSeq::Frame frame = qseq->frame(0);
for(int i=0; i < numJoints; ++i){
q1[i] = frame[i];
}
interpolator.clear();
interpolator.appendSample(0.0, q0);
interpolator.appendSample(2.0, q1);
interpolator.update();
qref_old = interpolator.interpolate(0.0);
currentFrame = 0;
phase = 0;
time = 0.0;
return true;
}
ForwardDynamicsABM::ForwardDynamicsABM(BodyPtr body) :
ForwardDynamics(body),
q0(body->numLinks()),
dq0(body->numLinks()),
dq(body->numLinks()),
ddq(body->numLinks())
{
}
virtual void input() override
{
if(ready){
dynamicsSimulator->getCharacterAllLinkData(
characterName.c_str(), DynamicsSimulator::JOINT_VALUE, q);
int n = std::min((int)q->length(), ioBody->numJoints());
for(int i=0; i < n; ++i){
ioBody->joint(i)->q() = q[i];
}
}
}
virtual void output() override
{
if(ready){
u.length(ioBody->numJoints());
for(int i=0; i < ioBody->numJoints(); ++i){
u[i] = ioBody->joint(i)->u();
}
dynamicsSimulator->setCharacterAllLinkData(
characterName.c_str(), DynamicsSimulator::JOINT_TORQUE, u);
}
}
void BodyState::set(BodyPtr i_body)
{
Link *root = i_body->rootLink();
p = root->p;
R = root->R;
q.resize(i_body->numJoints());
for (int i=0; i<i_body->numJoints(); i++){
Link *joint = i_body->joint(i);
if (joint){
q[i] = joint->q;
}
}
}
void BodyMotionPoseProvider::initialize(BodyPtr body__, BodyMotionPtr motion)
{
body_ = body__->clone();
this->motion = motion;
footLinkPositions.reset(new MultiAffine3Seq());
footLinks.clear();
ikPaths.clear();
LeggedBodyHelperPtr legged = getLeggedBodyHelper(body_);
if(legged->isValid()){
for(int i=0; i < legged->numFeet(); ++i){
Link* link = legged->footLink(i);
JointPathPtr ikPath = getCustomJointPath(body_, body_->rootLink(), link);
if(ikPath){
if(ikPath->hasAnalyticalIK() || ikPath->numJoints() == 6){
footLinks.push_back(link);
ikPaths.push_back(ikPath);
}
}
}
ZMP_ = legged->homeCopOfSoles();
} else {
ZMP_.setZero();
}
updateMotion();
}
void PoseSeq::store(Mapping& archive, const BodyPtr body) const
{
archive.write("type", "PoseSeq");
archive.write("name", name(), DOUBLE_QUOTED);
archive.write("targetBody", body->name(), DOUBLE_QUOTED);
Listing& refsNode = *archive.createListing("refs");
for(PoseRefList::const_iterator p = refs.begin(); p != refs.end(); ++p){
const PoseRef& ref = *p;
MappingPtr refNode = refsNode.newMapping();
refNode->write("time", ref.time());
if(ref.maxTransitionTime() > 0.0){
refNode->write("maxTransitionTime", ref.maxTransitionTime());
}
const string& name = ref.name();
if((storedNames.find(name) == storedNames.end() /* && !ref.isExternalReference()*/) ||
name.empty()){
const_cast<PoseSeq*>(this)->storedNames.insert(name);
MappingPtr childNode = refNode->createMapping("refer");
ref.poseUnit()->store(*childNode, body);
} else {
refNode->write("refer", name, DOUBLE_QUOTED);
}
}
}
void MeshShapeItemImpl::onUpdated()
{
sceneLink->translation() = self->absTranslation;
sceneLink->rotation() = self->absRotation;
if (path != ""){
if (shape) {
sceneLink->removeChild(shape);
shape = NULL;
}
BodyLoader bodyLoader;
BodyPtr newBody = bodyLoader.load(path);
if (!newBody) return;
shape = newBody->rootLink()->visualShape();
sceneLink->addChildOnce(shape);
sceneLink->notifyUpdate();
}
}
bool PoseSeq::restore(const Mapping& archive, const BodyPtr body)
{
setTargetBodyName(archive.get("targetBody", body->name()));
const Listing& refs = *archive.findListing("refs");
if(!refs.isValid()){
return false;
}
PoseSeq::iterator current = begin();
for(int i=0; i < refs.size(); ++i){
const Mapping& ref = *refs[i].toMapping();
bool isInserted = false;
double time = ref["time"].toDouble();
const ValueNode& referred = ref["refer"];
if(referred.isScalar()){
const string& name = referred;
if(!name.empty()){
current = insert(current, time, name);
isInserted = true;
}
} else if(referred.isMapping()){
const Mapping& mReferred = *referred.toMapping();
const string& type = mReferred["type"];
PoseUnitPtr poseUnit;
if(type == "Pose"){
poseUnit = new Pose();
} else if(type == "PronunSymbol"){
poseUnit = new PronunSymbol();
}
/*
else if(type == "PoseSeq"){
poseUnit = createLocalPoseSeq();
}
*/
if(poseUnit && poseUnit->restore(mReferred, body)){
poseUnit->name_ = mReferred["name"];
current = insert(current, time, poseUnit);
isInserted = true;
}
}
if(isInserted){
current->setMaxTransitionTime(ref.get("maxTransitionTime", 0.0));
}
}
return true;
}
void BodyLinkViewImpl::update()
{
currentLink = 0;
if(!currentBodyItem){
nameLabel.setText("");
return;
}
propertyWidgetConnections.block();
stateWidgetConnections.block();
BodyPtr body = currentBodyItem->body();
const vector<int>& selectedLinkIndices =
LinkSelectionView::mainInstance()->getSelectedLinkIndices(currentBodyItem);
if(selectedLinkIndices.empty()){
currentLink = body->rootLink();
} else {
currentLink = body->link(selectedLinkIndices.front());
}
if(currentLink){
nameLabel.setText(QString("%1 / %2").arg(body->name().c_str()).arg(currentLink->name().c_str()));
updateLink();
} else {
nameLabel.setText(body->name().c_str());
}
if(currentBodyItem->isLeggedBody()){
zmpBox.show();
} else {
zmpBox.hide();
}
updateKinematicState(false);
updateCollisions();
stateWidgetConnections.unblock();
propertyWidgetConnections.unblock();
}
void BodyBarImpl::onSymmetricCopyButtonClicked(int direction, bool doMirrorCopy)
{
for(size_t i=0; i < targetBodyItems.size(); ++i){
const Listing& slinks = *targetBodyItems[i]->body()->info()->findListing("symmetricJoints");
if(slinks.isValid() && !slinks.empty()){
targetBodyItems[i]->beginKinematicStateEdit();
int from = direction;
int to = 1 - direction;
BodyPtr body = targetBodyItems[i]->body();
for(int j=0; j < slinks.size(); ++j){
const Listing& linkPair = *slinks[j].toListing();
if(linkPair.size() == 1 && doMirrorCopy){
Link* link = body->link(linkPair[0].toString());
if(link){
link->q() = -link->q();
}
} else if(linkPair.size() >= 2){
Link* link1 = body->link(linkPair[from].toString());
Link* link2 = body->link(linkPair[to].toString());
if(link1 && link2){
double sign = 1.0;
if(linkPair.size() >= 3){
sign = linkPair[2].toDouble();
}
if(doMirrorCopy){
double q1 = link1->q();
link1->q() = sign * link2->q();
link2->q() = sign * q1;
} else {
link2->q() = sign * link1->q();
}
}
}
}
targetBodyItems[i]->notifyKinematicStateChange(true);
targetBodyItems[i]->acceptKinematicStateEdit();
}
}
}
void MultiAffine3SeqGraphView::setupGraphWidget()
{
graph.clearDataHandlers();
for(list<ItemInfo>::iterator it = itemInfos.begin(); it != itemInfos.end(); ++it){
if(it->bodyItem){
MultiAffine3SeqPtr seq = it->item->seq();
int numParts = seq->numParts();
BodyPtr body = it->bodyItem->body();
const std::vector<int>& selectedLinkIndices = linkSelection->getSelectedLinkIndices(it->bodyItem);
for(size_t i=0; i < selectedLinkIndices.size(); ++i){
Link* link = body->link(selectedLinkIndices[i]);
if(link && link->index < numParts){
addPositionTrajectory(it, link, seq);
}
}
}
}
}
virtual bool control()
{
BodyPtr io = ioBody();
if(cnt < 1000){
io->joint(0)->u() = 0.0;
io->joint(1)->u() = 0.0;
} else if(cnt < 3000){
io->joint(0)->u() = 1.5;
io->joint(1)->u() = 1.5;
} else if(cnt < 4000){
io->joint(0)->u() = 1.0;
io->joint(1)->u() = -1.0;
} else {
io->joint(0)->u() = 1.0;
io->joint(1)->u() = 1.0;
}
cnt++;
return true;
}
BodySymmetry::BodySymmetry(BodyPtr body)
: body(body)
{
const YamlSequence& sjoints = *body->info()->findSequence("symmetricJoints");
if(sjoints.isValid() && !sjoints.empty()){
for(int i=0; i < sjoints.size(); ++i){
const YamlSequence& jointPair = *sjoints[i].toSequence();
if(jointPair.size() == 1){
Link* link = body->link(jointPair[0].toString());
if(link){
JointSymmetry& symmetry = jointSymmetryMap[link->jointId];
symmetry.counterPartJointId = link->jointId;
symmetry.jointPositionSign = -1.0;
symmetry.offset = 0.0;
}
} else if(jointPair.size() >= 2){
Link* joint[2];
joint[0] = body->link(jointPair[0].toString());
joint[1] = body->link(jointPair[1].toString());
if(joint[0] && joint[1] && joint[0]->jointId >= 0 && joint[1]->jointId >= 0){
for(int j=0; j < 2; ++j){
int other = 1 - j;
JointSymmetry& symmetry = jointSymmetryMap[joint[j]->jointId];
symmetry.counterPartJointId = joint[other]->jointId;
symmetry.sign = (jointPair.size() >= 3) ? jointPair[2].toDouble() : 1.0;
symmetry.offset = (jointPair.size() >= 4) ? jointPair[3].toDouble() : 0.0;
}
}
}
}
}
const YamlSequence& slinks = *body->info()->findSequence("symmetricIkLinks");
if(slinks.isValid() && !slinks.empty()){
for(int i=0; i < slinks.size(); ++i){
const YamlSequence& linkPair = *slinks[i].toSequence();
if(linkPair.size() == 1){
Link* link = body->link(linkPair[0].toString());
if(link){
LinkSymmetry& symmetry = linkFlipMap[link->index];
symmetry.counterPartIndex = link->index;
}
} else if(linkPair.size() >= 2){
Link* link[2];
link[0] = body->link(linkPair[0].toString());
link[1] = body->link(linkPair[1].toString());
if(link[0] && link[1]){
for(int j=0; j < 2; ++j){
int other = 1 - j;
LinkSymmetry& symmetry = linkFlipMap[link[j]->index];
symmetry.counterPartIndex = link[other]->index;
}
}
}
}
}
}
virtual bool control() {
switch(phase){
case 0 :
qref = interpolator.interpolate(time);
if(time > interpolator.domainUpper()){
phase = 1;
}
break;
case 1:
if(currentFrame < qseq->numFrames()){
MultiValueSeq::Frame frame = qseq->frame(currentFrame++);
for(int i=0; i < numJoints; ++i){
qref[i] = frame[i];
}
}else{
phase = 2;
}
break;
case 2 :
qref = interpolator.interpolate(time);
}
for(int i=0; i < body->numJoints(); ++i){
Link* joint = body->joint(i);
double q = joint->q();
double dq_ref = (qref[i] - qref_old[i]) / timeStep_;
double dq = (q - qold[i]) / timeStep_;
joint->u() = (qref[i] - q) * pgain[i] + (dq_ref - dq) * dgain[i];
qold[i] = q;
}
qref_old = qref;
time += timeStep_;
return true;
}
bool EditableModelItemImpl::loadModelFile(const std::string& filename)
{
BodyPtr newBody;
MessageView* mv = MessageView::instance();
mv->beginStdioRedirect();
bodyLoader.setMessageSink(mv->cout(true));
newBody = bodyLoader.load(filename);
mv->endStdioRedirect();
if(newBody){
newBody->initializeState();
newBody->calcForwardKinematics();
Link* link = newBody->rootLink();
AbstractBodyLoaderPtr loader = bodyLoader.lastActualBodyLoader();
VRMLBodyLoader* vloader = dynamic_cast<VRMLBodyLoader*>(loader.get());
if (vloader) {
// VRMLBodyLoader supports retriveOriginalNode function
setLinkTree(link, vloader);
} else {
// Other loaders dont, so we wrap with inline node
VRMLProtoInstance* proto = new VRMLProtoInstance(new VRMLProto(""));
MFNode* children = new MFNode();
VRMLInlinePtr inl = new VRMLInline();
inl->urls.push_back(filename);
children->push_back(inl);
proto->fields["children"] = *children;
// first, create joint item
JointItemPtr item = new JointItem(link);
item->originalNode = proto;
self->addChildItem(item);
ItemTreeView::instance()->checkItem(item, true);
// next, create link item under the joint item
LinkItemPtr litem = new LinkItem(link);
litem->originalNode = proto;
litem->setName("link");
item->addChildItem(litem);
ItemTreeView::instance()->checkItem(litem, true);
}
for (int i = 0; i < newBody->numDevices(); i++) {
Device* dev = newBody->device(i);
SensorItemPtr sitem = new SensorItem(dev);
Item* parent = self->findItem<Item>(dev->link()->name());
if (parent) {
parent->addChildItem(sitem);
ItemTreeView::instance()->checkItem(sitem, true);
}
}
}
return (newBody);
}
void BasicSensorSimulationHelper::initialize
(BodyPtr body, double timeStep, const Vector3& gravityAcceleration)
{
isActive_ = false;
DeviceList<> devices = body->devices();
if(!devices.empty()){
forceSensors_.extractFrom(devices);
rateGyroSensors_.extractFrom(devices);
accelerationSensors_.extractFrom(devices);
if(!forceSensors_.empty() ||
!rateGyroSensors_.empty() ||
!accelerationSensors_.empty()){
impl->initialize(body, timeStep, gravityAcceleration);
isActive_ = true;
}
}
}
void gen_colmap(BodyPtr body, Link *link1, Link *link2)
{
JointPathPtr path = body->getJointPath(link1, link2);
if (path->numJoints() != 2){
std::cerr << "the number of joints between " << link1->name << " and "
<< link2->name << "isn't equal to 2" << std::endl;
return;
}
ColdetLinkPairPtr pair = new ColdetLinkPair(link1, link2);
Link *joint1=path->joint(0), *joint2=path->joint(1);
double th1 = joint1->llimit, th2;
#define DTH (M_PI/180);
std::string fname = link1->name + "_" + link2->name + ".dat";
std::ofstream ofs(fname.c_str());
ofs << link1->ulimit << " " << link2->ulimit << std::endl;
ofs << link1->ulimit << " " << link2->llimit << std::endl;
ofs << link1->llimit << " " << link2->llimit << std::endl;
ofs << link1->llimit << " " << link2->ulimit << std::endl;
ofs << link1->ulimit << " " << link2->ulimit << std::endl;
ofs << std::endl << std::endl;
int ntest=0, ncollision=0;
while (th1 < joint1->ulimit){
joint1->q = th1;
th2 = joint2->llimit;
while (th2 < joint2->ulimit){
joint2->q = th2;
path->calcForwardKinematics();
link1->coldetModel->setPosition(link1->attitude(), link1->p);
link2->coldetModel->setPosition(link2->attitude(), link2->p);
ntest++;
if (pair->checkCollision()){
ofs << th1 << " " << th2 << std::endl;
ncollision++;
}
th2 += DTH;
}
th1 += DTH;
}
std::cout << link1->name << "<->" << link2->name << ":" << ncollision
<< "/" << ntest << std::endl;
}
bool createBody(BodyPtr& body, BodyInfo_impl& bodyInfo)
{
this->body = body;
const char* name = bodyInfo.name();
body->setModelName(name);
body->setName(name);
int n = bodyInfo.links()->length();
linkInfoSeq = bodyInfo.links();
shapeInfoSeq = bodyInfo.shapes();
extraJointInfoSeq = bodyInfo.extraJoints();
int rootIndex = -1;
for(int i=0; i < n; ++i){
if(linkInfoSeq[i].parentIndex < 0){
if(rootIndex < 0){
rootIndex = i;
} else {
// more than one root !
rootIndex = -1;
break;
}
}
}
if(rootIndex >= 0){ // root exists
Matrix33 Rs(Matrix33::Identity());
Link* rootLink = createLink(rootIndex, Rs);
body->setRootLink(rootLink);
body->setDefaultRootPosition(rootLink->b, rootLink->Rs);
body->installCustomizer();
body->initializeConfiguration();
#if 0
setExtraJoints();
#endif
return true;
}
return false;
}
void BodyMotionPoseProvider::initialize(BodyPtr body__, BodyMotionPtr motion)
{
body_ = body__->duplicate();
this->motion = motion;
footLinkPositions.reset(new MultiAffine3Seq());
footLinks.clear();
ikPaths.clear();
YamlSequence& footLinkInfos = *body_->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());
JointPathPtr ikPath = body_->getJointPath(body_->rootLink(), link);
if(ikPath && ikPath->hasAnalyticalIK()){
footLinks.push_back(link);
ikPaths.push_back(ikPath);
}
}
}
updateMotion();
}
bool VRMLBodyLoader::load(BodyPtr body, const std::string& filename)
{
body->clearDevices();
body->clearExtraJoints();
return impl->load(body.get(), filename);
}
int main(int argc, char *argv[])
{
OnlineViewer_var olv;
olv = hrp::getOnlineViewer(argc, argv);
olv->load("robot", url);
olv->clearLog();
BodyPtr body = BodyPtr(new Body());
loadBodyFromModelLoader(body, url, argc, argv, true);
body->setName("robot");
convertToConvexHull(body);
WorldState wstate;
wstate.time = 0.0;
wstate.characterPositions.length(1);
setupCharacterPosition(wstate.characterPositions[0], body);
wstate.collisions.length(3);
// ワイヤの固定部
std::vector<Anchor> anchors;
anchors.push_back(Anchor(body->link("CHEST_Y"), Vector3(-0.03, 0.1,0.35)));
anchors.push_back(Anchor(body->link("R_HIP_P"), Vector3(-0.08,0,-0.2)));
anchors.push_back(Anchor(body->link("CHEST_Y"), Vector3(-0.03,-0.1,0.35)));
anchors.push_back(Anchor(body->link("L_HIP_P"), Vector3(-0.08,0,-0.2)));
wstate.collisions.length(anchors.size()+1);
for (size_t i=1; i<wstate.collisions.length(); i++){
setupFrame(wstate.collisions[i], 0.05);
}
// ワイヤと接触するリンクセットその1
std::vector<Link *> linkset1;
linkset1.push_back(body->link("CHEST_Y"));
linkset1.push_back(body->link("WAIST"));
linkset1.push_back(body->link("R_HIP_P"));
// ワイヤと接触するリンクセットその2
std::vector<Link *> linkset2;
linkset2.push_back(body->link("CHEST_Y"));
linkset2.push_back(body->link("WAIST"));
linkset2.push_back(body->link("L_HIP_P"));
// ワイヤ（両端点及びリンクセット）の定義
std::vector<String> strings;
strings.push_back(String(&anchors[0], &anchors[1], linkset1));
strings.push_back(String(&anchors[2], &anchors[3], linkset2));
std::ifstream ifs(logfile);
if (!ifs.is_open()){
std::cerr << "failed to open the log file:" << logfile << std::endl;
return 1;
}
char buf[1024];
ifs.getline(buf, 1024); // skip header
double q;
while(1){
for (int i=0; i<body->numJoints(); i++){
ifs >> q;
if (ifs.eof()) return 0;
Link *j = body->joint(i);
if (j) j->q = q;
}
ifs.getline(buf, 1024); // skip rest of the line
body->calcForwardKinematics();
for (size_t i=0; i<strings.size(); i++){
if (!strings[i].update()){
std::cerr << "failed to update string state" << std::endl;
}
}
// update body
updateCharacterPosition(wstate.characterPositions[0], body);
// update anchors
for (size_t i=0; i<anchors.size(); i++){
updateFrame(wstate.collisions[i+1],
anchors[i].position(), Matrix33::Identity());
}
// update strings
size_t n=0, index=0;
for (size_t i=0; i<strings.size(); i++){
n += strings[i].polyLine.lines.size();
}
wstate.collisions[0].points.length(n);
for (size_t i=0; i<strings.size(); i++){
const std::vector<LineSegment>& lines = strings[i].polyLine.lines;
for (size_t i=0; i<lines.size(); i++){
updateLineSegment(wstate.collisions[0].points[index++],
lines[i].first, lines[i].second);
}
}
olv->update(wstate);
wstate.time += 0.005;
std::cout << wstate.time << " ";
for (size_t i=0; i<strings.size(); i++){
std::cout << strings[i].length() << " ";
}
std::cout << std::endl;
}
return 0;
}
int KinematicFaultCheckerImpl::checkFaults
(BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os,
bool checkPosition, bool checkVelocity, bool checkCollision, dynamic_bitset<> linkSelection,
double beginningTime, double endingTime)
{
numFaults = 0;
BodyPtr body = bodyItem->body();
BodyMotionPtr motion = motionItem->motion();
MultiValueSeqPtr qseq = motion->jointPosSeq();;
MultiSE3SeqPtr pseq = motion->linkPosSeq();
if((!checkPosition && !checkVelocity && !checkCollision) || body->isStaticModel() || !qseq->getNumFrames()){
return numFaults;
}
BodyState orgKinematicState;
if(USE_DUPLICATED_BODY){
body = body->clone();
} else {
bodyItem->storeKinematicState(orgKinematicState);
}
CollisionDetectorPtr collisionDetector;
WorldItem* worldItem = bodyItem->findOwnerItem<WorldItem>();
if(worldItem){
collisionDetector = worldItem->collisionDetector()->clone();
} else {
int index = CollisionDetector::factoryIndex("AISTCollisionDetector");
if(index >= 0){
collisionDetector = CollisionDetector::create(index);
} else {
collisionDetector = CollisionDetector::create(0);
os << _("A collision detector is not found. Collisions cannot be detected this time.") << endl;
}
}
addBodyToCollisionDetector(*body, *collisionDetector);
collisionDetector->makeReady();
const int numJoints = std::min(body->numJoints(), qseq->numParts());
const int numLinks = std::min(body->numLinks(), pseq->numParts());
frameRate = motion->frameRate();
double stepRatio2 = 2.0 / frameRate;
angleMargin = radian(angleMarginSpin.value());
translationMargin = translationMarginSpin.value();
velocityLimitRatio = velocityLimitRatioSpin.value() / 100.0;
int beginningFrame = std::max(0, (int)(beginningTime * frameRate));
int endingFrame = std::min((motion->numFrames() - 1), (int)lround(endingTime * frameRate));
lastPosFaultFrames.clear();
lastPosFaultFrames.resize(numJoints, std::numeric_limits<int>::min());
lastVelFaultFrames.clear();
lastVelFaultFrames.resize(numJoints, std::numeric_limits<int>::min());
lastCollisionFrames.clear();
if(checkCollision){
Link* root = body->rootLink();
root->p().setZero();
root->R().setIdentity();
}
for(int frame = beginningFrame; frame <= endingFrame; ++frame){
int prevFrame = (frame == beginningFrame) ? beginningFrame : frame - 1;
int nextFrame = (frame == endingFrame) ? endingFrame : frame + 1;
for(int i=0; i < numJoints; ++i){
Link* joint = body->joint(i);
double q = qseq->at(frame, i);
joint->q() = q;
if(joint->index() >= 0 && linkSelection[joint->index()]){
if(checkPosition){
bool fault = false;
if(joint->isRotationalJoint()){
fault = (q > (joint->q_upper() - angleMargin) || q < (joint->q_lower() + angleMargin));
} else if(joint->isSlideJoint()){
fault = (q > (joint->q_upper() - translationMargin) || q < (joint->q_lower() + translationMargin));
}
if(fault){
putJointPositionFault(frame, joint, os);
}
}
if(checkVelocity){
double dq = (qseq->at(nextFrame, i) - qseq->at(prevFrame, i)) / stepRatio2;
joint->dq() = dq;
if(dq > (joint->dq_upper() * velocityLimitRatio) || dq < (joint->dq_lower() * velocityLimitRatio)){
putJointVelocityFault(frame, joint, os);
}
}
}
}
if(checkCollision){
Link* link = body->link(0);
if(!pseq->empty())
{
const SE3& p = pseq->at(frame, 0);
link->p() = p.translation();
link->R() = p.rotation().toRotationMatrix();
}
else
{
link->p() = Vector3d(0., 0., 0.);
link->R() = Matrix3d::Identity();
}
body->calcForwardKinematics();
for(int i=1; i < numLinks; ++i){
link = body->link(i);
if(!pseq->empty())
{
const SE3& p = pseq->at(frame, i);
link->p() = p.translation();
link->R() = p.rotation().toRotationMatrix();
}
}
for(int i=0; i < numLinks; ++i){
link = body->link(i);
collisionDetector->updatePosition(i, link->position());
}
collisionDetector->detectCollisions(
boost::bind(&KinematicFaultCheckerImpl::putSelfCollision, this, body.get(), frame, _1, boost::ref(os)));
}
}
if(!USE_DUPLICATED_BODY){
bodyItem->restoreKinematicState(orgKinematicState);
}
return numFaults;
}
virtual bool control() {
switch(phase){
case 0 :
qref = interpolator.interpolate(time);
if(time > interpolator.domainUpper()){
phase = 1;
}
break;
case 1:
if(currentFrame < qseq->numFrames()){
MultiValueSeq::Frame frame = qseq->frame(currentFrame++);
for(int i=0; i < numJoints; ++i){
qref[i] = frame[i];
}
}else{
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_shoulder_r] = -0.4;
q1[rarm_shoulder_p] = 0.75;
q1[rarm_elbow] = -2.0;
interpolator.appendSample(time + 3.0, q1);
q1[rarm_elbow] = -1.57;
q1[rarm_shoulder_p] = -0.2;
q1[rarm_wrist_r] = 1.5;
interpolator.appendSample(time + 5.0, q1);
q1[rarm_elbow] = -1.3;
q1[rarm_wrist_y] = -0.24;
interpolator.appendSample(time + 6.0, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 2;
}
break;
case 2 :
qref = interpolator.interpolate(time);
if(time > interpolator.domainUpper()){
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_wrist_y] = 0.0;
q1[rarm_shoulder_r] = 0.1;
interpolator.appendSample(time + 5.0, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 3;
}
break;
case 3:
qref = interpolator.interpolate(time);
if( rhsensor->F()[1] < -2 ) {
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_wrist_r] = -0.3;;
interpolator.appendSample(time + 2.0, q1);
interpolator.appendSample(time + 2.5, q1);
q1[rarm_shoulder_p] = -0.13;
q1[rarm_elbow] = -1.8;
interpolator.appendSample(time + 3.5, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 4;
}
break;
case 4 :
qref = interpolator.interpolate(time);
}
for(int i=0; i < body->numJoints(); ++i){
Link* joint = body->joint(i);
double q = joint->q();
double dq_ref = (qref[i] - qref_old[i]) / timeStep_;
double dq = (q - qold[i]) / timeStep_;
joint->u() = (qref[i] - q) * pgain[i] + (dq_ref - dq) * dgain[i];
qold[i] = q;
}
qref_old = qref;
time += timeStep_;
return true;
}
int main(int argc, char *argv[])
{
srand((unsigned)time(NULL));
const char *robotURL = NULL;
std::vector<std::string> obstacleURL;
std::vector<Vector3> obstacleP;
std::vector<Vector3> obstacleRpy;
for(int i = 1 ; i < argc; i++) {
if (strcmp(argv[i], "-robot") == 0) {
robotURL = argv[++i];
} else if (strcmp(argv[i], "-obstacle") == 0) {
obstacleURL.push_back(argv[++i]);
Vector3 p, rpy;
for (int j=0; j<3; j++) p[j] = atof(argv[++i]);
obstacleP.push_back(p);
for (int j=0; j<3; j++) rpy[j] = atof(argv[++i]);
obstacleRpy.push_back(rpy);
}
}
if (robotURL == NULL) {
std::cerr << "please specify URL of VRML model by -robot option"
<< std::endl;
return 1;
}
BodyPtr robot = new Body();
loadBodyFromModelLoader(robot, robotURL, argc, argv, true);
problem prob;
prob.addRobot("robot", robotURL, robot);
std::vector<BodyPtr> obstacles;
for (unsigned int i=0; i<obstacleURL.size(); i++) {
char buf[20];
sprintf(buf, "obstacle%02d", i);
obstacles.push_back(prob.addObstacle(buf, obstacleURL[i]));
}
// This must be called after all bodies are added
prob.initOLV(argc, argv);
PathEngine::Configuration::size(4+6+6);
PathEngine::Configuration::bounds(0, 0.2, 0.8); // body z
PathEngine::Configuration::bounds(1, -0.5, 0.5); // body roll
PathEngine::Configuration::bounds(2, -0.0, 0.5); // body pitch
PathEngine::Configuration::bounds(3, -0.5, 0.5); // body yaw
int arm;
PathEngine::Configuration goalCfg;
std::ifstream ifs("goal.txt");
ifs >> arm;
for (unsigned int i=0; i<PathEngine::Configuration::size(); i++) {
ifs >> goalCfg[i];
}
#if 1
PathEngine::Configuration::weight(0) = 0.1; // z
PathEngine::Configuration::weight(1) = 1; // roll
PathEngine::Configuration::weight(2) = 1; // pitch
PathEngine::Configuration::weight(3) = 1; // yaw
PathEngine::Configuration::weight(4) = 0.8;
PathEngine::Configuration::weight(5) = 0.6;
PathEngine::Configuration::weight(6) = 0.4;
PathEngine::Configuration::weight(7) = 0.3;
PathEngine::Configuration::weight(8) = 0.2;
PathEngine::Configuration::weight(9) = 0.1;
PathEngine::Configuration::weight(10) = 0.8;
PathEngine::Configuration::weight(11) = 0.6;
PathEngine::Configuration::weight(12) = 0.4;
PathEngine::Configuration::weight(13) = 0.3;
PathEngine::Configuration::weight(14) = 0.2;
PathEngine::Configuration::weight(15) = 0.1;
#endif
JointPathPtr armPath[2];
Link *chest = robot->link("CHEST_JOINT1");
Link *wrist[2] = {robot->link("RARM_JOINT5"),
robot->link("LARM_JOINT5")
};
for (int k=0; k<2; k++) {
armPath[k] = robot->getJointPath(chest, wrist[k]);
for (int i=0; i<armPath[k]->numJoints(); i++) {
Link *j = armPath[k]->joint(i);
PathEngine::Configuration::bounds(4+k*6+i, j->llimit, j->ulimit);
}
}
PathEngine::PathPlanner *planner = prob.planner();
planner->setMobilityName("OmniWheel");
planner->setAlgorithmName("RRT");
PathEngine::RRT *rrt = (PathEngine::RRT *)planner->getAlgorithm();
rrt->extendFromGoal(true);
planner->getAlgorithm()->setProperty("eps", "0.1");
planner->getAlgorithm()->setProperty("max-trials", "50000");
planner->getAlgorithm()->setProperty("interpolation-distance", "0.05");
prob.initCollisionCheckPairs();
prob.initPlanner();
for (unsigned int i=0; i<obstacles.size(); i++) {
Link *root = obstacles[i]->rootLink();
root->p = obstacleP[i];
root->R = rotFromRpy(obstacleRpy[i]);
root->coldetModel->setPosition(root->R, root->p);
obstacles[i]->calcForwardKinematics();
}
// set halfconf
dvector halfconf;
halfconf.setZero(robot->numJoints());
#define ToRad(x) ((x)*M_PI/180)
halfconf[2] = halfconf[ 8] = ToRad(-40);
halfconf[3] = halfconf[ 9] = ToRad( 78);
halfconf[4] = halfconf[10] = ToRad(-38);
double leg_link_len1=0, leg_link_len2=0;
halfconf[16] = halfconf[23] = ToRad(20);
halfconf[17] = ToRad(-10);
halfconf[24] = -halfconf[17];
halfconf[19] = halfconf[26] = ToRad(-30);
if (robot->modelName() == "HRP2") {
halfconf[20] = ToRad(80);
halfconf[27] = -halfconf[20];
halfconf[22] = halfconf[29] = -1.0;
}
leg_link_len1 = leg_link_len2 = 0.3;
double waistHeight = leg_link_len1*cos(halfconf[2])
+ leg_link_len2*cos(halfconf[4])
+ 0.105;
robot->rootLink()->p(2) = waistHeight;
for (int i=0; i<robot->numJoints(); i++) {
robot->joint(i)->q = halfconf[i];
}
robot->calcForwardKinematics();
myCfgSetter setter(robot);
PathEngine::Configuration startCfg;
startCfg[0] = robot->rootLink()->p[2];
startCfg[1] = startCfg[2] = startCfg[3] = 0;
for (int j=0; j<2; j++) {
for (int i=0; i<armPath[j]->numJoints(); i++) {
startCfg[4+j*6+i] = armPath[j]->joint(i)->q;
}
}
planner->setStartConfiguration(startCfg);
planner->setGoalConfiguration(goalCfg);
planner->setApplyConfigFunc(boost::bind(&myCfgSetter::set, &setter,
_1, _2));
planner->setConfiguration(startCfg);
prob.updateOLV();
planner->setConfiguration(goalCfg);
prob.updateOLV();
#if 0
int earm;
ifs >> earm;
while (!ifs.eof()) {
PathEngine::Configuration cfg;
for (unsigned int i=0; i<PathEngine::Configuration::size(); i++) {
ifs >> cfg[i];
}
if (arm == earm) {
rrt->addExtraGoal(cfg);
std::cout << "added an extra goal" << std::endl;
planner->setConfiguration(cfg);
prob.updateOLV();
}
ifs >> earm;
}
#endif
CustomCD cd(robot, "hrp2.shape", "hrp2.pairs",
obstacles[0], "plant.pc");
prob.planner()->setCollisionDetector(&cd);
struct timeval tv1, tv2;
gettimeofday(&tv1, NULL);
// plan
bool ret = planner->calcPath();
if (ret) {
for (int i=0; i<5; i++) {
planner->optimize("Shortcut");
planner->optimize("RandomShortcut");
}
}
gettimeofday(&tv2, NULL);
if (ret) {
const std::vector<PathEngine::Configuration>& postures
= planner->getWayPoints();
std::ofstream ofs("path.txt");
ofs << arm << std::endl;
for (unsigned int i=0; i<postures.size(); i++) {
planner->setConfiguration(postures[i]);
for (int j=0; j<3; j++) {
ofs << robot->rootLink()->p[j] << " ";
}
for (int j=0; j<3; j++) {
for (int k=0; k<3; k++) {
ofs << robot->rootLink()->R(j,k) << " ";
}
}
for (int j=0; j<robot->numJoints(); j++) {
ofs << robot->joint(j)->q << " ";
}
ofs << std::endl;
prob.updateOLV();
}
} else {
PathEngine::Roadmap *roadmap = planner->getRoadmap();
for (unsigned int i=0; i<roadmap->nNodes(); i++) {
PathEngine::RoadmapNode *node = roadmap->node(i);
planner->setConfiguration(node->position());
prob.updateOLV();
}
}
double time = (tv2.tv_sec - tv1.tv_sec) + ((double)(tv2.tv_usec - tv1.tv_usec))/1e6;
std::cout << "total time = " << time << "[s]" << std::endl;
double cdtime = prob.planner()->timeCollisionCheck();
std::cout << "time spent in collision detection:"
<< cdtime << "[s](" << cdtime/time*100 << "[%]), "
<< cdtime*1e3/prob.planner()->countCollisionCheck() << "[ms/call]" << std::endl;
std::cout << "profile:";
setter.profile();
return 0;
}
virtual bool control()
{
BodyPtr io = ioBody();
io->joint(0)->u() = -1.0;
return true;
}
