bool Leg::canStepTo(const Vec3f& p) {
ActLens a = solveIK(p);
ActLens b = solveIK(Vec3f(p.x, p.y, parent->getUpHeight()));
ActLens c = solveIK(Vec3f(p.x, p.y, parent->getDownHeight()));
return (canMoveTo(a) && canMoveTo(b) && canMoveTo(c));
}
void Animation::setRotation(BVHNode* node,double x,double y,double z)
{
if (node)
{
//qDebug(QString("Animation::setRotation(")+jointName+")");
for(int i=0;i<NUM_IK;i++)
{
if(ikOn[i])
{
solveIK();
break;
}
}
if(node->isKeyframe(frame))
node->setKeyframeRotation(frame,Rotation(x,y,z));
else
{
node->addKeyframe(frame,node->frameData(frame).position(),Rotation(x,y,z));
setEaseIn(node,frame,Settings::easeIn());
setEaseOut(node,frame,Settings::easeOut());
}
// node->dumpKeyframes();
BVHNode* mirrorNode=node->getMirror();
if(mirrored && mirrorNode)
{
// new keyframe system
if(mirrorNode->isKeyframe(frame))
mirrorNode->setKeyframeRotation(frame,Rotation(x,-y,-z));
else
{
mirrorNode->addKeyframe(frame,node->frameData(frame).position(),Rotation(x,-y,-z));
setEaseIn(mirrorNode,frame,Settings::easeIn());
setEaseOut(mirrorNode,frame,Settings::easeOut());
}
// tell timeline that this mirrored keyframe has changed (added or changed is the same here)
emit redrawTrack(getPartIndex(mirrorNode));
}
setDirty(true);
// tell timeline that this keyframe has changed (added or changed is the same here)
emit redrawTrack(getPartIndex(node));
emit frameChanged();
}
else
{
qDebug("Animaiton::setRotation(): node==0!");
}
}
bool Leg::setTarget(Vec3f p, bool force) {
ActLens acts = solveIK(p);
if(force || canMoveTo(acts)) {
target = p;
if(!remote) {
actuators[0].setTarget(acts.a, force);
actuators[1].setTarget(acts.b, force);
actuators[2].setTarget(acts.c, force);
}
else
RemoteManager.sendTarget(remotePort, remoteIndex, target);
return true;
}
else return false;
}
void Leg::synchronizeRemote(const Vec3f& remoteFoot){
if(!remote) {
// This is an incoming command. Call methods to change state.
setTarget(remoteFoot, true);
}
else {
this->foot = remoteFoot;
// Update actuator lengths based on foot
ActLens newLens = solveIK(foot);
actuators[0].setTarget(newLens.a);
actuators[0].update();
actuators[1].setTarget(newLens.b);
actuators[1].update();
actuators[2].setTarget(newLens.c);
actuators[2].update();
}
}
void Animation::setFrame(int frameNumber)
{
if(frameNumber>=0 && frameNumber<totalFrames && frame!=frameNumber)
{
// for (int i=0; i<NUM_IK; i++) {
// setIK((IKPartType)i, false);
// }
frame=frameNumber;
for(int i=0;i<NUM_IK;i++)
{
if(ikOn[i])
{
solveIK();
break;
}
}
emit currentFrame(frame);
emit frameChanged();
}
}
void Animation::setPosition(double x,double y,double z)
{
for(int i=0;i<NUM_IK;i++)
{
if(ikOn[i])
{
solveIK();
break;
}
}
// new keyframe system
if(positionNode->isKeyframe(frame))
positionNode->setKeyframePosition(frame,Position(x,y,z));
else
{
positionNode->addKeyframe(frame,Position(x,y,z),Rotation());
setEaseIn(positionNode,frame,Settings::easeIn());
setEaseOut(positionNode,frame,Settings::easeOut());
}
setDirty(true);
// tell timeline that this keyframe has changed (added or changed is the same here)
emit redrawTrack(0);
emit frameChanged();
}
MStatus ik2Bsolver::doSolve()
//
// This is the doSolve method which calls solveIK.
//
{
MStatus stat;
// Handle Group
//
MIkHandleGroup * handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
// Handle
//
// For single chain types of solvers, get the 0th handle.
// Single chain solvers are solvers which act on one handle only,
// i.e. the handle group for a single chain solver
// has only one handle
//
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle handleFn(handlePath, &stat);
// Effector
//
MDagPath effectorPath;
handleFn.getEffector(effectorPath);
MFnIkEffector effectorFn(effectorPath);
// Mid Joint
//
effectorPath.pop();
MFnIkJoint midJointFn(effectorPath);
// Start Joint
//
MDagPath startJointPath;
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn(startJointPath);
// Preferred angles
//
double startJointPrefAngle[3];
double midJointPrefAngle[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJointFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle,
startJointFn.rotationOrder());
midJointFn.setRotation(midJointPrefAngle,
midJointFn.rotationOrder());
AwPoint handlePos = handleFn.rotatePivot(MSpace::kWorld);
AwPoint effectorPos = effectorFn.rotatePivot(MSpace::kWorld);
AwPoint midJointPos = midJointFn.rotatePivot(MSpace::kWorld);
AwPoint startJointPos = startJointFn.rotatePivot(MSpace::kWorld);
AwVector poleVector = poleVectorFromHandle(handlePath);
poleVector *= handlePath.exclusiveMatrix();
double twistValue = twistFromHandle(handlePath);
AwQuaternion qStart, qMid;
solveIK(startJointPos,
midJointPos,
effectorPos,
handlePos,
poleVector,
twistValue,
qStart,
qMid);
midJointFn.rotateBy(qMid, MSpace::kWorld);
startJointFn.rotateBy(qStart, MSpace::kWorld);
return MS::kSuccess;
}
void doubleTouchThread::run()
{
skinContactList *skinContacts = skinPort -> read(false);
sendOutput();
if (checkMotionDone())
{
switch (step)
{
case 0:
steerArmsHome();
yInfo("[doubleTouch] WAITING FOR CONTACT...\n");
step++;
break;
case 1:
if(skinContacts)
{
printMessage(2,"Waiting for contact..\n");
if (detectContact(skinContacts))
{
yInfo("[doubleTouch] CONTACT!!! skinPart: %s Link: %i Position: %s NormDir: %s",
SkinPart_s[cntctSkinPart].c_str(), cntctLinkNum,cntctPosLink.toString(3,3).c_str(),
cntctNormDir.toString(3,3).c_str());
printMessage(1,"Switching to impedance position mode..\n");
imodeS -> setInteractionMode(2,VOCAB_IM_COMPLIANT);
imodeS -> setInteractionMode(3,VOCAB_IM_COMPLIANT);
step++;
}
}
break;
case 2:
solveIK();
yInfo("[doubleTouch] Going to taxel... Desired EE: %s\n",(sol->ee).toString(3,3).c_str());
printMessage(1,"Desired joint configuration: %s\n",(sol->joints*iCub::ctrl::CTRL_RAD2DEG).toString(3,3).c_str());
step++;
recFlag = 1;
break;
case 3:
configureHands();
if (record != 0)
{
Time::delay(2.0);
}
if (curTaskType == "LHtoR" || curTaskType == "RHtoL")
{
goToTaxelMaster();
}
else
{
goToTaxelSlave();
}
step++;
break;
case 4:
Time::delay(2.0);
step++;
break;
case 5:
if (curTaskType == "LHtoR" || curTaskType == "RHtoL")
{
goToTaxelSlave();
}
else
{
goToTaxelMaster();
}
step++;
break;
case 6:
recFlag = 0;
bool flag;
if (record == 0)
{
Time::delay(3.0);
flag=1;
if (flag == 1)
{
testAchievement();
step += 2;
}
}
else
{
testAchievement();
printMessage(0,"Waiting for the event to go back.\n");
step++;
}
break;
case 7:
if(skinContacts)
{
if (record == 1)
{
if (testAchievement2(skinContacts))
step++;
else if (robot == "icub" && exitFromDeadlock(skinContacts))
step++;
}
else if (record == 2)
{
if (exitFromDeadlock(skinContacts))
step++;
else
testAchievement2(skinContacts);
}
}
break;
case 8:
if (!dontgoback)
{
printMessage(0,"Going to rest...\n");
clearTask();
steerArmsHomeMasterSlave();
step++;
}
break;
case 9:
printMessage(1,"Switching to position mode..\n");
imodeS -> setInteractionMode(2,VOCAB_IM_STIFF);
imodeS -> setInteractionMode(3,VOCAB_IM_STIFF);
yInfo("[doubleTouch] WAITING FOR CONTACT...\n");
step = 1;
break;
default:
yError("[doubleTouch] doubleTouchThread should never be here!!!\nStep: %d",step);
Time::delay(2.0);
break;
}
}
}
MStatus ik2Bsolver::doSolve()
//
// This is the doSolve method which calls solveIK.
//
{
MStatus stat;
// Handle Group
//
MIkHandleGroup * handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
// Handle
//
// For single chain types of solvers, get the 0th handle.
// Single chain solvers are solvers which act on one handle only,
// i.e. the handle group for a single chain solver
// has only one handle
//
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle handleFn(handlePath, &stat);
// Effector
//
MDagPath effectorPath;
handleFn.getEffector(effectorPath);
// MGlobal::displayInfo(effectorPath.fullPathName());
MFnIkEffector effectorFn(effectorPath);
// Mid Joint
//
effectorPath.pop();
MFnIkJoint midJointFn(effectorPath);
// End Joint
//
MDagPath endJointPath;
bool hasEndJ = findFirstJointChild(effectorPath, endJointPath);
// if(hasEndJ) MGlobal::displayInfo(endJointPath.fullPathName());
MFnIkJoint endJointFn(endJointPath);
// Start Joint
//
MDagPath startJointPath;
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn(startJointPath);
// Preferred angles
//
double startJointPrefAngle[3];
double midJointPrefAngle[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJointFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle,
startJointFn.rotationOrder());
midJointFn.setRotation(midJointPrefAngle,
midJointFn.rotationOrder());
MPoint handlePos = handleFn.rotatePivot(MSpace::kWorld);
MPoint effectorPos = effectorFn.rotatePivot(MSpace::kWorld);
MPoint midJointPos = midJointFn.rotatePivot(MSpace::kWorld);
MPoint startJointPos = startJointFn.rotatePivot(MSpace::kWorld);
MVector poleVector = poleVectorFromHandle(handlePath);
poleVector *= handlePath.exclusiveMatrix();
double twistValue = twistFromHandle(handlePath);
MObject thisNode = thisMObject();
MVector localEndJointT = endJointFn.getTranslation(MSpace::kTransform);
MVector worldEndJointT = endJointFn.rotatePivot(MSpace::kWorld) - midJointPos;
double scaling = worldEndJointT.length() / ( localEndJointT.length() + 1e-6);
// MGlobal::displayInfo(MString(" scaling ")+scaling);
// get rest length
//
double restL1, restL2;
MPlug(thisNode, arestLength1).getValue(restL1);
MPlug(thisNode, arestLength2).getValue(restL2);
// get soft distance
//
MPlug plug( thisNode, asoftDistance );
double softD = 0.0;
plug.getValue(softD);
restL1 *= scaling;
restL2 *= scaling;
softD *= scaling;
// get max stretching
double maxStretching = 0.0;
MPlug(thisNode, amaxStretching).getValue(maxStretching);
MQuaternion qStart, qMid;
double stretching = 0.0;
solveIK(startJointPos,
midJointPos,
effectorPos,
handlePos,
poleVector,
twistValue,
qStart,
qMid,
softD,
restL1, restL2,
stretching);
midJointFn.rotateBy(qMid, MSpace::kWorld);
startJointFn.rotateBy(qStart, MSpace::kWorld);
midJointFn.setTranslation(MVector(restL1 / scaling, 0.0, 0.0), MSpace::kTransform);
endJointFn.setTranslation(MVector(restL2 / scaling, 0.0, 0.0), MSpace::kTransform);
// if(stretching > maxStretching) stretching = maxStretching;
if(maxStretching > 0.0) {
MVector vstretch(stretching* 0.5 / scaling, 0.0, 0.0);
midJointFn.translateBy(vstretch, MSpace::kTransform);
endJointFn.translateBy(vstretch, MSpace::kTransform);
}
return MS::kSuccess;
}
bool Leg::canMoveTo(const Vec3f& p) {
ActLens a = solveIK(p);
return canMoveTo(a);
}
std::vector<GraspScored> Reaching::selectFeasibleGrasps(const agile_grasp::Grasps& grasps_in)
{
std::vector<GraspScored> grasps_selected;
// evaluate the reachability of each grasp
for (int i = 0; i < grasps_in.grasps.size(); i++)
{
const agile_grasp::Grasp& grasp = grasps_in.grasps[i];
// check whether grasp lies within the workspace of the robot arm
ROS_INFO_COND(params_.is_printing_, "Checking if grasp %i, position (%1.2f, %1.2f, %1.2f), can be reached: ", i,
grasp.center.x, grasp.center.y, grasp.center.z);
if (!isInWorkspace(grasp.surface_center.x, grasp.surface_center.y, grasp.surface_center.z))
{
ROS_INFO_COND(params_.is_printing_, " NOT OK!");
continue;
}
ROS_INFO_COND(params_.is_printing_, " OK");
// avoid objects that are smaller/larger than the minimum/maximum robot hand aperture
ROS_INFO_COND(params_.is_printing_, "Checking aperture: ");
if (grasp.width.data < params_.min_aperture_ || grasp.width.data > params_.max_aperture_)
{
ROS_INFO_COND(params_.is_printing_, "too small/large for the hand (min, max): %.4f (%.4f, %.4f)!",
grasp.width.data, params_.min_aperture_, params_.max_aperture_);
continue;
}
ROS_INFO_COND(params_.is_printing_, " OK");
GraspEigen grasp_eigen(grasp);
// generate additional grasps
Eigen::VectorXd theta;
if (params_.num_additional_grasps_ > 0)
{
theta = Eigen::VectorXd::LinSpaced(1 + params_.num_additional_grasps_, -15.0, 15.0);
}
else
{
theta.resize(1);
theta << 0.0;
}
// check all grasps for reachability
for (int j = 0; j < theta.size(); j++)
{
ROS_INFO_COND(params_.is_printing_, "j: %i", j);
// calculate approach vector and hand axis for the new grasp
GraspEigen grasp_eigen_rot = rotateGrasp(grasp_eigen, theta[j]);
// create a grasp for each hand orientation and check whether they are reachable by the IK and collision-free
std::vector<tf::Quaternion> quats = calculateHandOrientations(grasp_eigen_rot);
bool is_collision_free = false;
for (int k = 0; k < quats.size(); k++)
{
ROS_INFO_COND(params_.is_printing_, "k: %i", k);
// create grasp pose
geometry_msgs::PoseStamped grasp_pose = createGraspPose(grasp_eigen_rot, quats[k], theta[j]);
// try to solve IK
ROS_INFO_COND(params_.is_printing_, " Solving IK: ");
double tik0 = omp_get_wtime();
IKSolution ik_solution = solveIK(grasp_pose);
ROS_INFO_COND(params_.is_printing_, " IK runtime: %.2f", omp_get_wtime() - tik0);
if (!ik_solution.success_) // IK fails
{
ROS_INFO_COND(params_.is_printing_, "IK failed for grasp %i, approach %i, orientation %i!\n", i, j, k);
continue;
}
ROS_INFO_COND(params_.is_printing_, " OK");
// check collisions (only required for one orientation/quaternion)
ROS_INFO_COND(params_.is_printing_, " Checking collisions: ");
if (!is_collision_free)
{
double tcoll0 = omp_get_wtime();
is_collision_free = isCollisionFree(grasp_pose, grasp_eigen_rot.approach_);
ROS_INFO_COND(params_.is_printing_, " Collision checker runtime: %.2f", omp_get_wtime() - tcoll0);
if (!is_collision_free)
{
ROS_INFO_COND(params_.is_printing_, "Grasp %i, approach %i, orientation %i collides with point cloud!\n", i,
j, k);
continue;
}
}
ROS_INFO_COND(params_.is_printing_, " OK");
if (params_.is_printing_)
{
std::cout << "IK solution: ";
for(int t=0; t < ik_solution.joint_positions_.size(); t++)
std::cout << ik_solution.joint_positions_[t] << " ";
std::cout << std::endl;
}
// create grasp based on inverse kinematics solution
GraspScored grasp_scored(i, grasp_pose, grasp_eigen_rot.approach_, grasp.width.data, ik_solution.joint_positions_, 0.0);
grasps_selected.push_back(grasp_scored);
}
}
}
return grasps_selected;
}
