supports slide joints in calcCMJacobian()

fkanehiro · Apr 3, 2016 · 7b674f8 · 7b674f8
1 parent b9b1b85
commit 7b674f8
Showing 1 changed file with 6 additions and 0 deletions.
diff --git a/hrplib/hrpModel/Body.cpp b/hrplib/hrpModel/Body.cpp
@@ -900,6 +900,12 @@ void Body::calcCMJacobian(Link *base, dmatrix &J)
             J.col(j->jointId) = dp;
             break;
         }
+	case Link::SLIDE_JOINT:
+	{
+	  Vector3 dp((j->subm/totalMass_)*sgn[j->jointId]*j->R*j->d);
+	  J.col(j->jointId) = dp;
+	  break;
+	}
         default:
             std::cerr << "calcCMJacobian() : unsupported jointType("
                       << j->jointType << ")" << std::endl;