void drawDemo (Graphics& g) override
{
g.setColour (Colours::black.withAlpha (getAlpha()));
glyphs.draw (g, getTransform());
}
/**
* @param rect the new size of the container
* @param invalid the views to dirty
*/
void CViewContainer::setViewSize (const CRect &rect, bool invalid)
{
if (rect == getViewSize ())
return;
CRect oldSize (getViewSize ());
CView::setViewSize (rect, invalid);
if (getAutosizingEnabled ())
{
CCoord widthDelta = rect.getWidth () - oldSize.getWidth ();
CCoord heightDelta = rect.getHeight () - oldSize.getHeight ();
getTransform ().inverse ().transform (widthDelta, heightDelta);
if (widthDelta != 0 || heightDelta != 0)
{
uint32_t numSubviews = getNbViews ();
uint32_t counter = 0;
bool treatAsColumn = (getAutosizeFlags () & kAutosizeColumn) != 0;
bool treatAsRow = (getAutosizeFlags () & kAutosizeRow) != 0;
FOREACHSUBVIEW
int32_t autosize = pV->getAutosizeFlags ();
CRect viewSize (pV->getViewSize ());
CRect mouseSize (pV->getMouseableArea ());
if (treatAsColumn)
{
if (counter)
{
viewSize.offset (counter * (widthDelta / (numSubviews)), 0);
mouseSize.offset (counter * (widthDelta / (numSubviews)), 0);
}
viewSize.setWidth (viewSize.getWidth () + (widthDelta / (numSubviews)));
mouseSize.setWidth (mouseSize.getWidth () + (widthDelta / (numSubviews)));
}
else if (widthDelta != 0 && autosize & kAutosizeRight)
{
viewSize.right += widthDelta;
mouseSize.right += widthDelta;
if (!(autosize & kAutosizeLeft))
{
viewSize.left += widthDelta;
mouseSize.left += widthDelta;
}
}
if (treatAsRow)
{
if (counter)
{
viewSize.offset (0, counter * (heightDelta / (numSubviews)));
mouseSize.offset (0, counter * (heightDelta / (numSubviews)));
}
viewSize.setHeight (viewSize.getHeight () + (heightDelta / (numSubviews)));
mouseSize.setHeight (mouseSize.getHeight () + (heightDelta / (numSubviews)));
}
else if (heightDelta != 0 && autosize & kAutosizeBottom)
{
viewSize.bottom += heightDelta;
mouseSize.bottom += heightDelta;
if (!(autosize & kAutosizeTop))
{
viewSize.top += heightDelta;
mouseSize.top += heightDelta;
}
}
if (viewSize != pV->getViewSize ())
{
pV->setViewSize (viewSize);
pV->setMouseableArea (mouseSize);
}
counter++;
ENDFOREACHSUBVIEW
}
}
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
if (!we_have_a_map)
{
ROS_INFO("SnapMapICP waiting for map to be published");
return;
}
ros::Time scan_in_time = scan_in->header.stamp;
ros::Time time_received = ros::Time::now();
if ( scan_in_time - last_processed_scan < ros::Duration(1.0f / SCAN_RATE) )
{
ROS_DEBUG("rejected scan, last %f , this %f", last_processed_scan.toSec() ,scan_in_time.toSec());
return;
}
//projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_);
if (!scan_callback_mutex.try_lock())
return;
ros::Duration scan_age = ros::Time::now() - scan_in_time;
//check if we want to accept this scan, if its older than 1 sec, drop it
if (!use_sim_time)
if (scan_age.toSec() > AGE_THRESHOLD)
{
//ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD);
ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold) scan time: %f , now %f", scan_age.toSec(), AGE_THRESHOLD, scan_in_time.toSec(),ros::Time::now().toSec() );
scan_callback_mutex.unlock();
return;
}
count_sc_++;
//ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_);
//if (count_sc_ > 10)
//if (count_sc_ > 10)
{
count_sc_ = 0;
tf::StampedTransform base_at_laser;
if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time))
{
ROS_WARN("Did not get base pose at laser scan time");
scan_callback_mutex.unlock();
return;
}
sensor_msgs::PointCloud cloud;
sensor_msgs::PointCloud cloudInMap;
projector_->projectLaser(*scan_in,cloud);
we_have_a_scan = false;
bool gotTransform = false;
if (!listener_->waitForTransform("/map", cloud.header.frame_id, cloud.header.stamp, ros::Duration(0.05)))
{
scan_callback_mutex.unlock();
ROS_WARN("SnapMapICP no map to cloud transform found MUTEX UNLOCKED");
return;
}
if (!listener_->waitForTransform("/map", "/base_link", cloud.header.stamp, ros::Duration(0.05)))
{
scan_callback_mutex.unlock();
ROS_WARN("SnapMapICP no map to base transform found MUTEX UNLOCKED");
return;
}
while (!gotTransform && (ros::ok()))
{
try
{
gotTransform = true;
listener_->transformPointCloud ("/map",cloud,cloudInMap);
}
catch (...)
{
gotTransform = false;
ROS_WARN("DIDNT GET TRANSFORM IN A");
}
}
for (size_t k =0; k < cloudInMap.points.size(); k++)
{
cloudInMap.points[k].z = 0;
}
gotTransform = false;
tf::StampedTransform oldPose;
while (!gotTransform && (ros::ok()))
{
try
{
gotTransform = true;
listener_->lookupTransform("/map", "/base_link",
cloud.header.stamp , oldPose);
}
catch (tf::TransformException ex)
{
gotTransform = false;
ROS_WARN("DIDNT GET TRANSFORM IN B");
}
}
if (we_have_a_map && gotTransform)
{
sensor_msgs::convertPointCloudToPointCloud2(cloudInMap,cloud2);
we_have_a_scan = true;
actScan++;
//pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> reg;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
reg.setTransformationEpsilon (1e-6);
// Set the maximum distance between two correspondences (src<->tgt) to 10cm
// Note: adjust this based on the size of your datasets
reg.setMaxCorrespondenceDistance(0.5);
reg.setMaximumIterations (ICP_NUM_ITER);
// Set the point representation
//ros::Time bef = ros::Time::now();
PointCloudT::Ptr myMapCloud (new PointCloudT());
PointCloudT::Ptr myScanCloud (new PointCloudT());
pcl::fromROSMsg(*output_cloud,*myMapCloud);
pcl::fromROSMsg(cloud2,*myScanCloud);
reg.setInputCloud(myScanCloud);
reg.setInputTarget(myMapCloud);
PointCloudT unused;
int i = 0;
reg.align (unused);
const Eigen::Matrix4f &transf = reg.getFinalTransformation();
tf::Transform t;
matrixAsTransfrom(transf,t);
//ROS_ERROR("proc time %f", (ros::Time::now() - bef).toSec());
we_have_a_scan_transformed = false;
PointCloudT transformedCloud;
pcl::transformPointCloud (*myScanCloud, transformedCloud, reg.getFinalTransformation());
double inlier_perc = 0;
{
// count inliers
std::vector<int> nn_indices (1);
std::vector<float> nn_sqr_dists (1);
size_t numinliers = 0;
for (size_t k = 0; k < transformedCloud.points.size(); ++k )
{
if (mapTree->radiusSearch (transformedCloud.points[k], ICP_INLIER_DIST, nn_indices,nn_sqr_dists, 1) != 0)
numinliers += 1;
}
if (transformedCloud.points.size() > 0)
{
//ROS_INFO("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD);
inlier_perc = (double) numinliers / (double) transformedCloud.points.size();
}
}
last_processed_scan = scan_in_time;
pcl::toROSMsg (transformedCloud, cloud2transformed);
we_have_a_scan_transformed = true;
double dist = sqrt((t.getOrigin().x() * t.getOrigin().x()) + (t.getOrigin().y() * t.getOrigin().y()));
double angleDist = t.getRotation().getAngle();
tf::Vector3 rotAxis = t.getRotation().getAxis();
t = t * oldPose;
tf::StampedTransform base_after_icp;
if (!getTransform(base_after_icp, ODOM_FRAME, "base_link", ros::Time(0)))
{
ROS_WARN("Did not get base pose at now");
scan_callback_mutex.unlock();
return;
}
else
{
tf::Transform rel = base_at_laser.inverseTimes(base_after_icp);
ROS_DEBUG("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI);
t= t * rel;
}
//ROS_DEBUG("dist %f angleDist %f",dist, angleDist);
//ROS_DEBUG("SCAN_AGE seems to be %f", scan_age.toSec());
char msg_c_str[2048];
sprintf(msg_c_str,"INLIERS %f (%f) scan_age %f (%f age_threshold) dist %f angleDist %f axis(%f %f %f) fitting %f (icp_fitness_threshold %f)",inlier_perc, ICP_INLIER_THRESHOLD, scan_age.toSec(), AGE_THRESHOLD ,dist, angleDist, rotAxis.x(), rotAxis.y(), rotAxis.z(),reg.getFitnessScore(), ICP_FITNESS_THRESHOLD );
std_msgs::String strmsg;
strmsg.data = msg_c_str;
//ROS_DEBUG("%s", msg_c_str);
double cov = POSE_COVARIANCE_TRANS;
//if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD))
// if ( reg.getFitnessScore() <= ICP_FITNESS_THRESHOLD )
if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (inlier_perc > ICP_INLIER_THRESHOLD) && (angleDist < ANGLE_UPPER_THRESHOLD))
{
lastTimeSent = actScan;
geometry_msgs::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
pose.pose.pose.position.x = t.getOrigin().x();
pose.pose.pose.position.y = t.getOrigin().y();
tf::Quaternion quat = t.getRotation();
//quat.setRPY(0.0, 0.0, theta);
tf::quaternionTFToMsg(quat,pose.pose.pose.orientation);
float factorPos = 0.03;
float factorRot = 0.1;
pose.pose.covariance[6*0+0] = (cov * cov) * factorPos;
pose.pose.covariance[6*1+1] = (cov * cov) * factorPos;
pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot;
ROS_DEBUG("i %i converged %i SCORE: %f", i, reg.hasConverged (), reg.getFitnessScore() );
ROS_DEBUG("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f [frame=%s]",dist, angleDist , pose.pose.pose.position.x , pose.pose.pose.position.y , pose.header.frame_id.c_str());
pub_pose.publish(pose);
strmsg.data += " << SENT";
}
//ROS_INFO("processing time : %f", (ros::Time::now() - time_received).toSec());
pub_info_.publish(strmsg);
//ROS_DEBUG("map width %i height %i size %i, %s", myMapCloud.width, myMapCloud.height, (int)myMapCloud.points.size(), myMapCloud.header.frame_id.c_str());
//ROS_DEBUG("scan width %i height %i size %i, %s", myScanCloud.width, myScanCloud.height, (int)myScanCloud.points.size(), myScanCloud.header.frame_id.c_str());
}
}
scan_callback_mutex.unlock();
}
void ScanMatcher::getPoseDifference(const tf::Transform& other, float_t& position_difference, float_t& orientation_difference) const {
position_difference = getTransform().getOrigin().distance(other.getOrigin());
orientation_difference = getTransform().getRotation().angleShortestPath(other.getRotation());
}
void Label::draw(sf::RenderTarget& target, sf::RenderStates states) const
{
states.transform *= getTransform();
target.draw(mText, states);
}
Point3F Etherform::_move( const F32 travelTime, Collision *outCol )
{
// Try and move to new pos
F32 totalMotion = 0.0f;
// TODO: not used?
//F32 initialSpeed = mVelocity.len();
Point3F start;
Point3F initialPosition;
getTransform().getColumn(3,&start);
initialPosition = start;
static CollisionList collisionList;
static CollisionList physZoneCollisionList;
collisionList.clear();
physZoneCollisionList.clear();
MatrixF collisionMatrix(true);
collisionMatrix.setColumn(3, start);
VectorF firstNormal(0.0f, 0.0f, 0.0f);
F32 time = travelTime;
U32 count = 0;
static Polyhedron sBoxPolyhedron;
static ExtrudedPolyList sExtrudedPolyList;
static ExtrudedPolyList sPhysZonePolyList;
for (; count < sMoveRetryCount; count++) {
F32 speed = mVelocity.len();
if(!speed)
break;
Point3F end = start + mVelocity * time;
Point3F distance = end - start;
if (mFabs(distance.x) < mObjBox.len_x() &&
mFabs(distance.y) < mObjBox.len_y() &&
mFabs(distance.z) < mObjBox.len_z())
{
// We can potentially early out of this. If there are no polys in the clipped polylist at our
// end position, then we can bail, and just set start = end;
Box3F wBox = mScaledBox;
wBox.minExtents += end;
wBox.maxExtents += end;
static EarlyOutPolyList eaPolyList;
eaPolyList.clear();
eaPolyList.mNormal.set(0.0f, 0.0f, 0.0f);
eaPolyList.mPlaneList.clear();
eaPolyList.mPlaneList.setSize(6);
eaPolyList.mPlaneList[0].set(wBox.minExtents,VectorF(-1.0f, 0.0f, 0.0f));
eaPolyList.mPlaneList[1].set(wBox.maxExtents,VectorF(0.0f, 1.0f, 0.0f));
eaPolyList.mPlaneList[2].set(wBox.maxExtents,VectorF(1.0f, 0.0f, 0.0f));
eaPolyList.mPlaneList[3].set(wBox.minExtents,VectorF(0.0f, -1.0f, 0.0f));
eaPolyList.mPlaneList[4].set(wBox.minExtents,VectorF(0.0f, 0.0f, -1.0f));
eaPolyList.mPlaneList[5].set(wBox.maxExtents,VectorF(0.0f, 0.0f, 1.0f));
// Build list from convex states here...
CollisionWorkingList& rList = mConvex.getWorkingList();
CollisionWorkingList* pList = rList.wLink.mNext;
while (pList != &rList) {
Convex* pConvex = pList->mConvex;
if (pConvex->getObject()->getTypeMask() & sCollisionMoveMask) {
Box3F convexBox = pConvex->getBoundingBox();
if (wBox.isOverlapped(convexBox))
{
// No need to separate out the physical zones here, we want those
// to cause a fallthrough as well...
pConvex->getPolyList(&eaPolyList);
}
}
pList = pList->wLink.mNext;
}
if (eaPolyList.isEmpty())
{
totalMotion += (end - start).len();
start = end;
break;
}
}
collisionMatrix.setColumn(3, start);
sBoxPolyhedron.buildBox(collisionMatrix, mScaledBox, true);
// Setup the bounding box for the extrudedPolyList
Box3F plistBox = mScaledBox;
collisionMatrix.mul(plistBox);
Point3F oldMin = plistBox.minExtents;
Point3F oldMax = plistBox.maxExtents;
plistBox.minExtents.setMin(oldMin + (mVelocity * time) - Point3F(0.1f, 0.1f, 0.1f));
plistBox.maxExtents.setMax(oldMax + (mVelocity * time) + Point3F(0.1f, 0.1f, 0.1f));
// Build extruded polyList...
VectorF vector = end - start;
sExtrudedPolyList.extrude(sBoxPolyhedron,vector);
sExtrudedPolyList.setVelocity(mVelocity);
sExtrudedPolyList.setCollisionList(&collisionList);
sPhysZonePolyList.extrude(sBoxPolyhedron,vector);
sPhysZonePolyList.setVelocity(mVelocity);
sPhysZonePolyList.setCollisionList(&physZoneCollisionList);
// Build list from convex states here...
CollisionWorkingList& rList = mConvex.getWorkingList();
CollisionWorkingList* pList = rList.wLink.mNext;
while (pList != &rList) {
Convex* pConvex = pList->mConvex;
if (pConvex->getObject()->getTypeMask() & sCollisionMoveMask) {
Box3F convexBox = pConvex->getBoundingBox();
if (plistBox.isOverlapped(convexBox))
{
if (pConvex->getObject()->getTypeMask() & PhysicalZoneObjectType)
pConvex->getPolyList(&sPhysZonePolyList);
else
pConvex->getPolyList(&sExtrudedPolyList);
}
}
pList = pList->wLink.mNext;
}
// Take into account any physical zones...
for (U32 j = 0; j < physZoneCollisionList.getCount(); j++)
{
AssertFatal(dynamic_cast<PhysicalZone*>(physZoneCollisionList[j].object), "Bad phys zone!");
const PhysicalZone* pZone = (PhysicalZone*)physZoneCollisionList[j].object;
if (pZone->isActive())
mVelocity *= pZone->getVelocityMod();
}
if (collisionList.getCount() != 0 && collisionList.getTime() < 1.0f)
{
// Set to collision point
F32 velLen = mVelocity.len();
F32 dt = time * getMin(collisionList.getTime(), 1.0f);
start += mVelocity * dt;
time -= dt;
totalMotion += velLen * dt;
// Back off...
if ( velLen > 0.f ) {
F32 newT = getMin(0.01f / velLen, dt);
start -= mVelocity * newT;
totalMotion -= velLen * newT;
}
// Pick the surface most parallel to the face that was hit.
const Collision *collision = &collisionList[0];
const Collision *cp = collision + 1;
const Collision *ep = collision + collisionList.getCount();
for (; cp != ep; cp++)
{
if (cp->faceDot > collision->faceDot)
collision = cp;
}
F32 bd = -mDot(mVelocity, collision->normal);
// Copy this collision out so
// we can use it to do impacts
// and query collision.
*outCol = *collision;
// Subtract out velocity
VectorF dv = collision->normal * (bd + sNormalElasticity);
mVelocity += dv;
if (count == 0)
{
firstNormal = collision->normal;
}
else
{
if (count == 1)
{
// Re-orient velocity along the crease.
if (mDot(dv,firstNormal) < 0.0f &&
mDot(collision->normal,firstNormal) < 0.0f)
{
VectorF nv;
mCross(collision->normal,firstNormal,&nv);
F32 nvl = nv.len();
if (nvl)
{
if (mDot(nv,mVelocity) < 0.0f)
nvl = -nvl;
nv *= mVelocity.len() / nvl;
mVelocity = nv;
}
}
}
}
}
else
{
totalMotion += (end - start).len();
start = end;
break;
}
}
if (count == sMoveRetryCount)
{
// Failed to move
start = initialPosition;
mVelocity.set(0.0f, 0.0f, 0.0f);
}
return start;
}
void Tile::draw(sf::RenderTarget& target, sf::RenderStates states) const{
states.transform *= getTransform();
target.draw(rect, states);
}
void Animation::draw(sf::RenderTarget& target, sf::RenderStates states) const
{
states.transform *= getTransform();
target.draw(mSprite, states);
}
SpriteMovement::SpriteMovement() : data(SpriteMovementMotor::defaultSpriteData()) {
state_machine = std::unique_ptr<SpriteMovementMotor::SpriteFSM>(new SpriteMovementMotor::SpriteFSM(data, SpriteMovementMotor::FACE_DOWN, this));
data->transform = getTransform();
data->current_level = 0;
}
void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(odometry_->getPose().isNull() &&
!groundTruthFrameId_.empty())
{
// sync with the first value of the ground truth
Transform initialPose = getTransform(groundTruthFrameId_, frameId_, stamp);
if(initialPose.isNull())
{
return;
}
ROS_INFO("Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
frameId_.c_str());
odometry_->reset(initialPose);
}
// process data
ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info;
SensorData dataCpy = data;
rtabmap::Transform pose = odometry_->process(dataCpy, &info);
if(!pose.isNull())
{
//*********************
// Update odometry
//*********************
geometry_msgs::TransformStamped poseMsg;
poseMsg.child_frame_id = frameId_;
poseMsg.header.frame_id = odomFrameId_;
poseMsg.header.stamp = stamp;
rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);
if(publishTf_)
{
tfBroadcaster_.sendTransform(poseMsg);
}
if(odomPub_.getNumSubscribers())
{
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
//set the position
odom.pose.pose.position.x = poseMsg.transform.translation.x;
odom.pose.pose.position.y = poseMsg.transform.translation.y;
odom.pose.pose.position.z = poseMsg.transform.translation.z;
odom.pose.pose.orientation = poseMsg.transform.rotation;
//set covariance
odom.pose.covariance.at(0) = info.variance; // xx
odom.pose.covariance.at(7) = info.variance; // yy
odom.pose.covariance.at(14) = info.variance; // zz
odom.pose.covariance.at(21) = info.variance; // rr
odom.pose.covariance.at(28) = info.variance; // pp
odom.pose.covariance.at(35) = info.variance; // yawyaw
//set velocity
if(previousStamp_.isValid())
{
float x,y,z,roll,pitch,yaw;
odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
odom.twist.twist.linear.x = x;
odom.twist.twist.linear.y = y;
odom.twist.twist.linear.z = z;
odom.twist.twist.angular.x = roll;
odom.twist.twist.angular.y = pitch;
odom.twist.twist.angular.z = yaw;
}
previousStamp_ = stamp;
//publish the message
odomPub_.publish(odom);
}
// local map / reference frame
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
{
pcl::PointCloud<pcl::PointXYZ> cloud;
const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{
cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z));
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLocalMap_.publish(cloudMsg);
}
if(odomLastFrame_.getNumSubscribers())
{
if(dynamic_cast<OdometryF2M*>(odometry_))
{
const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(words3.size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{
// transform to odom frame
cv::Point3f pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
}
else
{
//Frame to Frame
const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
if(refFrame.getWords3().size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
{
// transform to odom frame
cv::Point3f pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
}
}
}
else
{
//ROS_WARN("Odometry lost!");
//send null pose to notify that odometry is lost
nav_msgs::Odometry odom;
odom.header.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
//publish the message
odomPub_.publish(odom);
}
if(odomInfoPub_.getNumSubscribers())
{
rtabmap_ros::OdomInfo infoMsg;
odomInfoToROS(info, infoMsg);
infoMsg.header.stamp = stamp; // use corresponding time stamp to image
infoMsg.header.frame_id = odomFrameId_;
odomInfoPub_.publish(infoMsg);
}
ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
}
void TiledScreen::drawUpper(sf::RenderWindow *window) {
this->renderStates.transform = getTransform();
for (int16_t l = 0; l < UP_LAYERS; l++)
window->draw(this->up[l], MAP_WIDTH*MAP_HEIGTH*VERS_PER_TILE, sf::Quads,this->renderStates);
}
void AnimatedSphere::draw(sf::RenderTarget& tgt, sf::RenderStates states) const noexcept
{
states.transform *= getTransform();
tgt.draw(circle, states);
}
void gui::MessageBox::draw(sf::RenderTarget& target,
sf::RenderStates states) const
{
states.transform *= getTransform();
mPages.at(mCurrentPage).draw(target, states);
}
void PrimitiveShader_PC::Flush()
{
if(buffer_.empty())
return;
auto renderer = getRenderer();
Use();
CheckGL();
// Send our transformation to the currently bound shader,
// in the "MVP" uniform
auto const &pv = renderer->getCamera().getPV();
glEnable(GL_BLEND);
if(hasTransform()){
//m[0][2],m[1][2] is always 0
auto const &m = getTransform();
auto pv2 = pv * Mat4(Vec4(m[0][0],m[0][1],0,m[0][2]),
Vec4(m[1][0],m[1][1],0,m[1][2]),
Vec4(0,0,1,0),
Vec4(m[2][0],m[2][1],0,m[2][2]));
glUniformMatrix4fv(pv_, 1, GL_FALSE, &pv2[0][0]);
}else{
glUniformMatrix4fv(pv_, 1, GL_FALSE, &pv[0][0]);
}
CheckGL();
glBindBuffer(GL_ARRAY_BUFFER, vbo_);
glBufferData(GL_ARRAY_BUFFER, sizeof(Point)*buffer_.size(), (const void*)&buffer_[0], GL_DYNAMIC_DRAW);
CheckGL();
// 1rst attribute buffer : vertices
glEnableVertexAttribArray(model_xy_);
glVertexAttribPointer(
model_xy_, // attribute. No particular reason for 0, but must match the layout in the shader.
2, // size
GL_FLOAT, // type
GL_FALSE, // normalized?
sizeof(Point), // stride
(void*)0 // array buffer offset
);
CheckGL();
// 2nd attribute buffer : color
glEnableVertexAttribArray(color_);
glVertexAttribPointer(
color_,
4,
GL_FLOAT,
GL_FALSE,
sizeof(Point),
(void*)(2*sizeof(GLfloat))
);
CheckGL();
glEnable(GL_PROGRAM_POINT_SIZE);
glDrawArrays((GLenum)drawMode_, 0, buffer_.size());
glDisable(GL_PROGRAM_POINT_SIZE);
CheckGL();
glDisableVertexAttribArray(model_xy_);
glDisableVertexAttribArray(color_);
glDisable(GL_BLEND);
Unuse();
buffer_.clear();
renderer->addRenderCall();
}
bool Etherform::updatePos(const F32 travelTime)
{
PROFILE_SCOPE(Etherform_UpdatePos);
getTransform().getColumn(3, &delta.posVec);
// When mounted to another object, only Z rotation used.
if(isMounted())
{
mVelocity = mMount.object->getVelocity();
setPosition(Point3F(0.0f, 0.0f, 0.0f), mRot);
setMaskBits(MoveMask);
return true;
}
Point3F newPos;
Collision col;
dMemset(&col, 0, sizeof(col));
// DEBUG:
//Point3F savedVelocity = mVelocity;
if(mVelocity.isZero())
newPos = delta.posVec;
else
newPos = _move(travelTime, &col);
_handleCollision(col);
// DEBUG:
//if ( isClientObject() )
// Con::printf( "(client) vel: %g %g %g", mVelocity.x, mVelocity.y, mVelocity.z );
//else
// Con::printf( "(server) vel: %g %g %g", mVelocity.x, mVelocity.y, mVelocity.z );
// Set new position
// If on the client, calc delta for backstepping
if(isClientObject())
{
delta.pos = newPos;
delta.rot = mRot;
delta.posVec = delta.posVec - delta.pos;
delta.rotVec = delta.rotVec - delta.rot;
delta.dt = 1.0f;
}
this->setPosition(newPos, mRot);
this->setMaskBits(MoveMask);
this->updateContainer();
if (!isGhost())
{
// Collisions are only queued on the server and can be
// generated by either updateMove or updatePos
notifyCollision();
// Do mission area callbacks on the server as well
//checkMissionArea();
}
// Check the total distance moved. If it is more than 1000th of the velocity, then
// we moved a fair amount...
//if (totalMotion >= (0.001f * initialSpeed))
return true;
//else
//return false;
}
void SpriteMovement::onStart() {
data->tile_location = glm::ivec2(getTransform()->getAbsoluteTranslation().x, -getTransform()->getAbsoluteTranslation().y);
data->tile_location += glm::ivec2(data->collision_data->getWidth() / 2, data->collision_data->getHeight() / 2 - 1);
state_machine->start();
}
void Etherform::processTick(const Move* move)
{
Parent::processTick(move);
// Warp to catch up to server?
if(delta.warpTicks > 0)
{
delta.warpTicks--;
// Set new pos.
getTransform().getColumn(3,&delta.pos);
delta.pos += delta.warpOffset;
delta.rot += delta.rotOffset;
this->setPosition(delta.pos,delta.rot);
this->setRenderPosition(delta.pos,delta.rot);
// Backstepping
delta.posVec.x = -delta.warpOffset.x;
delta.posVec.y = -delta.warpOffset.y;
delta.posVec.z = -delta.warpOffset.z;
delta.rotVec.x = -delta.rotOffset.x;
delta.rotVec.y = -delta.rotOffset.y;
delta.rotVec.z = -delta.rotOffset.z;
}
else
{
// If there is no move, the we're either an
// unattached etherform on the server, or another etherform's
// client ghost.
if(!move)
{
if(isGhost())
{
// If we haven't run out of prediction time,
// predict using the last known move.
if (mPredictionCount-- <= 0)
return;
move = &delta.move;
}
else
move = &NullMove;
}
if(isServerObject() || (didRenderLastRender() || getControllingClient()))
{
delta.move = *move;
if(isMounted())
{
// If we're mounted then do not perform any collision checks
// and clear our previous working list.
mConvex.clearWorkingList();
}
else
{
this->updateWorkingCollisionSet();
}
this->updateVelocity(move);
VectorF contactNormal(0,0,0);
this->findContact(&contactNormal );
this->updatePos();
}
}
// Add node to lasertrails.
if(this->isClientObject())
{
GameConnection* conn = GameConnection::getConnectionToServer();
if(conn && this == conn->getControlObject())
{
if(move->sendCount == 0)
this->addLaserTrailNode(this->getPosition());
}
else
this->addLaserTrailNode(this->getPosition());
}
}
sf::FloatRect Text::getGlobalBounds() const
{
return getTransform().transformRect(getLocalBounds());
}
void Etherform::_findContact( SceneObject **contactObject,
VectorF *contactNormal,
Vector<SceneObject*> *outOverlapObjects )
{
Point3F pos;
getTransform().getColumn(3,&pos);
Box3F wBox;
Point3F exp(0,0,sTractionDistance);
wBox.minExtents = pos + mScaledBox.minExtents - exp;
wBox.maxExtents.x = pos.x + mScaledBox.maxExtents.x;
wBox.maxExtents.y = pos.y + mScaledBox.maxExtents.y;
wBox.maxExtents.z = pos.z + mScaledBox.minExtents.z + sTractionDistance;
static ClippedPolyList polyList;
polyList.clear();
polyList.doConstruct();
polyList.mNormal.set(0.0f, 0.0f, 0.0f);
polyList.setInterestNormal(Point3F(0.0f, 0.0f, -1.0f));
polyList.mPlaneList.setSize(6);
polyList.mPlaneList[0].setYZ(wBox.minExtents, -1.0f);
polyList.mPlaneList[1].setXZ(wBox.maxExtents, 1.0f);
polyList.mPlaneList[2].setYZ(wBox.maxExtents, 1.0f);
polyList.mPlaneList[3].setXZ(wBox.minExtents, -1.0f);
polyList.mPlaneList[4].setXY(wBox.minExtents, -1.0f);
polyList.mPlaneList[5].setXY(wBox.maxExtents, 1.0f);
Box3F plistBox = wBox;
// Expand build box as it will be used to collide with items.
// PickupRadius will be at least the size of the box.
F32 pd = 0;
wBox.minExtents.x -= pd; wBox.minExtents.y -= pd;
wBox.maxExtents.x += pd; wBox.maxExtents.y += pd;
wBox.maxExtents.z = pos.z + mScaledBox.maxExtents.z;
// Build list from convex states here...
CollisionWorkingList& rList = mConvex.getWorkingList();
CollisionWorkingList* pList = rList.wLink.mNext;
while (pList != &rList)
{
Convex* pConvex = pList->mConvex;
U32 objectMask = pConvex->getObject()->getTypeMask();
if ( ( objectMask & sCollisionMoveMask ) &&
!( objectMask & PhysicalZoneObjectType ) )
{
Box3F convexBox = pConvex->getBoundingBox();
if (plistBox.isOverlapped(convexBox))
pConvex->getPolyList(&polyList);
}
else
outOverlapObjects->push_back( pConvex->getObject() );
pList = pList->wLink.mNext;
}
if (!polyList.isEmpty())
{
// Pick flattest surface
F32 bestVd = -1.0f;
ClippedPolyList::Poly* poly = polyList.mPolyList.begin();
ClippedPolyList::Poly* end = polyList.mPolyList.end();
for (; poly != end; poly++)
{
F32 vd = poly->plane.z; // i.e. mDot(Point3F(0,0,1), poly->plane);
if (vd > bestVd)
{
bestVd = vd;
*contactObject = poly->object;
*contactNormal = poly->plane;
}
}
}
}
sf::FloatRect AnimatedSprite::getGlobalBounds() const
{
return getTransform().transformRect(getLocalBounds());
}
//private
void LineSegment::draw(sf::RenderTarget& rt, sf::RenderStates states) const
{
states.transform *= getTransform();
if(m_texture) states.texture = m_texture;
rt.draw(m_vertexArray, states);
}
bool HoverVehicle::onNewDataBlock(GameBaseData* dptr, bool reload)
{
mDataBlock = dynamic_cast<HoverVehicleData*>(dptr);
if (!mDataBlock || !Parent::onNewDataBlock(dptr,reload))
return false;
if (isGhost())
{
// Create the sounds ahead of time. This reduces runtime
// costs and makes the system easier to understand.
SFX_DELETE( mEngineSound );
SFX_DELETE( mFloatSound );
SFX_DELETE( mJetSound );
if ( mDataBlock->sound[HoverVehicleData::EngineSound] )
mEngineSound = SFX->createSource( mDataBlock->sound[HoverVehicleData::EngineSound], &getTransform() );
if ( !mDataBlock->sound[HoverVehicleData::FloatSound] )
mFloatSound = SFX->createSource( mDataBlock->sound[HoverVehicleData::FloatSound], &getTransform() );
if ( mDataBlock->sound[HoverVehicleData::JetSound] )
mJetSound = SFX->createSource( mDataBlock->sound[HoverVehicleData::JetSound], &getTransform() );
}
// Todo: Uncomment if this is a "leaf" class
scriptOnNewDataBlock();
return true;
}
void Firebug::draw(sf::RenderTarget& target, sf::RenderStates states) const
{
states.transform *= getTransform();
// target.draw(mSprite,states);
mAnimation.draw(target,states);
}
void Joystick::render(sf::RenderTarget& target, sf::RenderStates states)
{
states.transform *= getTransform();
target.draw(mBackground,states);
target.draw(mButton,states);
}
void ParticleSkinnedModel::update(float dt)
{
// Send update call to superclass
Model::update(dt);
const int PS_SUB_UPDATE_STEPS = 2;
const int PS_CONSTRAINT_STEPS = 0;
const float TARGET_TIME = (float)(1.0 / (60.0 * PS_SUB_UPDATE_STEPS));
dt = TARGET_TIME;
if(m_simulateOnGPU)
{
int loc;
Shader& shader = m_ps->getShader();
shader.bind();
loc = shader.getUniformLocation("modelMatrix");
glUniformMatrix4fv(loc, 1, false, glm::value_ptr(getTransform().getMat4()));
loc = shader.getUniformLocation("invModelMatrix");
glUniformMatrix4fv(loc, 1, false, glm::value_ptr(getTransform().getInvMat4()));
loc = shader.getUniformLocation("boneMatrix");
calculateAndSetBoneMatrices(loc);
//loc = shader.getUniformLocation("randomForce");
for(int i=0; i<PS_SUB_UPDATE_STEPS; ++i)
{
m_ps->update(dt);
}
}
else
{
const size_t max_bones = 128;
glm::mat4 finalMat[max_bones];
// 16K on the stack thanks to this, perhaps allocate in heap?
// The idéa is to make sure it is coherent in memory.
std::vector<glm::vec3> targetPos(m_particles.size());
size_t boneCount = m_body->getBoneCount();
assert(boneCount < max_bones);
calculateFinalBoneMatrices(finalMat, boneCount);
for(size_t i=0; i<targetPos.size(); ++i)
{
const Body::sVertex& v = m_body->getVertexData()[i];
glm::vec4 finalPos(0.0f);
for(size_t u=0; u<MAX_WEIGHTS; ++u)
{
if(v.weight[u].unused())
break;
int index = v.weight[u].getIndex();
float weight = v.weight[u].getWeight();
finalPos += weight * finalMat[index] * glm::vec4(v.position, 1);
}
targetPos[i] = glm::vec3(finalPos);
}
const glm::vec3 externalForce(0);
const glm::vec3 externalAcc(0,-9.82,0);
const float maxDist = 0.1;
for(int u=0; u<PS_SUB_UPDATE_STEPS; ++u)
{
// Simulate timesteps
for(size_t i=0; i<m_particles.size(); ++i)
{
sParticle& p = m_particles[i];
glm::vec3& target = targetPos[i];
float mass = p.mass_k_d.x;
float k = p.mass_k_d.y;
float d = p.mass_k_d.z * 0.4f;
//float len = glm::length(target - p.position);
glm::vec3 attrForce = (target - p.position) * k;
glm::vec3 force = externalForce + attrForce;
glm::vec3 acc = externalAcc + force / mass;
glm::vec3 pos = p.position;
glm::vec3 vel = (1.0f - d) * (p.position - p.oldPosition) + acc * dt * dt;
// This part can probably be optimized further
// This is to make a soft clamp of the particle pos to the max allowed distance
float dist = glm::distance(pos, target);
glm::vec3 goPath = (pos-target) / dist;
pos += vel;
if(dist < maxDist) {
p.oldPosition = p.position;
p.position = pos;
}else{
glm::vec3 maxDistPos = target + goPath * maxDist;
p.position = glm::mix(pos, maxDistPos, glm::clamp(glm::distance(maxDistPos,pos), 0.0f, 1.0f));
p.oldPosition = p.position - vel;
}
}
}
const std::vector<Body::sVertex>& vertexData = m_body->getVertexData();
const float stiffness = 0.1f;
for(int u=0; u<PS_CONSTRAINT_STEPS; ++u)
{
// Update constraints
for(size_t i=0; i<m_constraints.size(); ++i)
{
size_t i0 = m_constraints[i].index[0];
size_t i1 = m_constraints[i].index[1];
sParticle& p0 = m_particles[i0];
sParticle& p1 = m_particles[i1];
float m0 = p0.mass_k_d.x;
float m1 = p1.mass_k_d.x;
glm::vec3 delta = p1.position - p0.position;
glm::vec3 restDelta = vertexData[i1].position - vertexData[i0].position;
float restLength2 = glm::dot(restDelta, restDelta);
delta *= (1.0f - 2.0f * restLength2 / (restLength2 + glm::dot(delta,delta)) ) * (m0 + m1);
glm::vec3 val = stiffness * delta;
p0.position += val / m0;
p1.position -= val / m1;
}
// Merge seems and maintain velocity
for(size_t i=0; i<m_postVertexMerge.size(); ++i)
{
const unsigned int i0 = m_postVertexMerge[i].index[0];
const unsigned int i1 = m_postVertexMerge[i].index[1];
glm::vec3 v0 = m_particles[i0].position - m_particles[i0].oldPosition;
glm::vec3 v1 = m_particles[i1].position - m_particles[i1].oldPosition;
glm::vec3 p = m_particles[i0].position * 0.5f + m_particles[i1].position * 0.5f;
m_particles[i0].position = p;
m_particles[i0].oldPosition = p - v0;
m_particles[i1].position = p;
m_particles[i1].oldPosition = p - v1;
}
}
// Update bufferdata!
m_particleBuffer.bind();
glBufferSubData(GL_ARRAY_BUFFER, 0, sizeof(sParticle) * m_particles.size(), &m_particles[0]);
m_particleBuffer.unbind();
}
}
void GuiComponent::render(const Eigen::Affine3f& parentTrans)
{
Eigen::Affine3f trans = parentTrans * getTransform();
renderChildren(trans);
}
FloatRect Shape::getGlobalBounds() const
{
return getTransform().transformRect(getLocalBounds());
}
Rectf Sprite::getGlobalBounds() const {
return getTransform().transformRect(getLocalBounds());
}
void CosmoButton::draw(sf::RenderTarget& target, sf::RenderStates states) const
{
states.transform *= getTransform();
target.draw(m_background, states);
target.draw((gui::Button) *this);
}
/*!
Sets up the chain given a XML DOM from a currently open robot
configuration file. It reads the number of joints and the number of links
and allocates space for those vectors, then it reads in the base transform
of the chain. Next, it reads a node for each joint and creates a
prismatic or revolute joint which is initialized with the kinematic data
in that node. \a linkDir should be the path to the directory where the link
body files are kept (usually rootPath/iv).
*/
int
KinematicChain::initChainFromXml(const TiXmlElement* root,QString &linkDir)
{
numJoints = countXmlElements(root,"joint");
if (numJoints < 1) {
DBGA("number of joints < 1");
return FAILURE;
}
numLinks = countXmlElements(root,"link");
if (numLinks < 1) {
DBGA("Number of links < 1");
return FAILURE;
}
jointVec.resize(numJoints, NULL);
linkVec.resize(numLinks, NULL);
lastJoint = new int[numLinks];
IVRoot = new SoSeparator;
IVTran = new SoTransform;
IVTran->ref();
/* read in the finger transformation */
const TiXmlElement* element = findXmlElement(root,"transform");
if(element){
if(!getTransform(element,tran)){
QTWARNING("Fail to perform transformation");
return FAILURE;
}
}
tran.toSoTransform(IVTran);
DBGA(" Creating joints");
numDOF = 0;
std::list<const TiXmlElement*> elementList = findAllXmlElements(root, "joint");
std::list<const TiXmlElement*>::iterator p;
int j;
for(p = elementList.begin(), j=0; p!=elementList.end(); p++,j++){
DBGA(" Joint " << j);
QString type = (*p)->Attribute("type");
if(type.isNull()){
QTWARNING("No Joint Type Specified");
return FAILURE;
}
if(type == "Revolute"){
jointVec[j] = new RevoluteJoint(this);
} else if(type == "Prismatic"){
jointVec[j] = new PrismaticJoint (this);
} else {
DBGA("Unknown joint type requested");
return FAILURE;
}
if (jointVec[j]->initJointFromXml(*p, firstJointNum+j) == FAILURE) {
DBGA("Failed to initialize joint");
return FAILURE;
}
}
DBGA(" Creating links");
std::vector<int> dynJointTypes;
int lastJointNum = -1;
elementList = findAllXmlElements(root, "link");
int l;
for (l=0, p=elementList.begin(); p!=elementList.end(); p++,l++){
DBGA(" Link " << l);
QString jointType=(*p)->Attribute("dynamicJointType");
if(jointType.isNull()){
QTWARNING("No Dynamic Joint Type Specified");
return FAILURE;
}
jointType = jointType.stripWhiteSpace();
if (jointType == "Revolute") {
dynJointTypes.push_back(DynJoint::REVOLUTE);
lastJointNum += 1;
} else if (jointType == "Ball") {
dynJointTypes.push_back(DynJoint::BALL);
lastJointNum += 3;
} else if (jointType == "Universal") {
dynJointTypes.push_back(DynJoint::UNIVERSAL);
lastJointNum += 2;
} else if (jointType == "Prismatic") {
dynJointTypes.push_back(DynJoint::PRISMATIC);
lastJointNum += 1;
} else if (jointType == "Fixed") {
dynJointTypes.push_back(DynJoint::FIXED);
} else {
DBGA("Unknown joint type requested");
return FAILURE;
}
QString linkFilename = (*p)->GetText();
linkFilename = linkFilename.stripWhiteSpace();
QString linkName = QString(owner->name()) + QString("_chain%1_link%2").arg(chainNum).arg(l);
linkVec[l] = new Link(owner,chainNum,l,owner->getWorld(),linkName.latin1());
if (linkVec[l]->load(linkDir + linkFilename)==FAILURE) {
delete linkVec[l]; linkVec[l] = NULL;
DBGA("Failed to load file for link " << l);
return FAILURE;
}
/*Handle collision rule settings:
*Common case is NULL - collisions are on except for adjacent links.
*Off - turn off collisions for this link globally, do not enter this body
*into the collision engine at all.
*OverlappingPair - turn off collisions between two particular links
*/
QString collisionRule=(*p)->Attribute("collisionRule");
if(!collisionRule.isNull()){
collisionRule = collisionRule.stripWhiteSpace();
if (collisionRule == "Off"){
linkVec[l]->addToIvc(true);
linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],NULL);
DBGA("Collisions off for link " << l);
}else if (collisionRule == "OverlappingPair"){
/*targetChain - specifies the chain of the target link to disable
*collisions for. No attribute implies current chain.
*Base implies robot base.
*targetLink - specifies link number in target chain
*/
linkVec[l]->addToIvc();
QString targetChain=(*p)->Attribute("targetChain");
targetChain = targetChain.stripWhiteSpace();
if (targetChain == "base")
linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],owner->getBase());
else{
QString targetLink = (*p)->Attribute("targetLink");
if (!targetLink.isNull()){
bool ok = TRUE;
int linkNum = targetLink.toInt(&ok);
if (!ok){
DBGA("targetLink not a valid input");
return FAILURE;
}
if(targetChain.isNull())
linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],linkVec[linkNum]);
else{
int chainNum = targetChain.toInt(&ok);
if (!ok){
DBGA("targetChain not a valid input");
return FAILURE;
}
linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],owner->getChain(chainNum)->linkVec[linkNum]);
}
}
}
}
else{
DBGA("Unknown Collision Rule");
return FAILURE;
}
}else{
linkVec[l]->addToIvc();
}
lastJoint[l] = lastJointNum;
if (lastJoint[l] >= numJoints) {
DBGA("Wrong last joint value: " << lastJoint[l]);
return FAILURE;
}
IVRoot->addChild(linkVec[l]->getIVRoot());
}
DBGA(" Creating dynamic joints");
if (createDynamicJoints(dynJointTypes) == FAILURE) {
DBGA("Failed to create dynamic joints");
return FAILURE;
}
jointsMoved = true;
updateLinkPoses();
owner->getWorld()->tendonChange();
owner->getIVRoot()->addChild(IVRoot);
return SUCCESS;
}