make modification based on Andres comments

mcx · Nov 10, 2014 · a00442c · a00442c
1 parent ff04ba2
commit a00442c
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 16 deletions.
diff --git a/solvers/trajectoryOptimization/GraspFrictionConeWrench.m b/solvers/trajectoryOptimization/GraspFrictionConeWrench.m
@@ -9,7 +9,7 @@
     finger_grasp_pt % The position of the grasping point on the finger, in finger frame
     grasp_object_FC_axis % A 3 x 1 vector. The normal direction in the GRASP OBJECT frame
     FC_mu % The friction coefficient
-    force_normalize_factor   % A positive double scaler. The actual force is force_normalize_factor*force_parameter,
+    force_normalization_factor   % A positive double scaler. The actual force is force_normalization_factor*force_parameter,
                              % where force parameters are the arguments being constrained
   end
 
@@ -18,7 +18,7 @@
   end
 
   methods
-    function obj = GraspFrictionConeWrench(robot,grasp_object_idx, finger_idx, finger_grasp_pt,grasp_object_FC_axis,FC_mu,force_normalize_factor)
+    function obj = GraspFrictionConeWrench(robot,grasp_object_idx, finger_idx, finger_grasp_pt,grasp_object_FC_axis,FC_mu,force_normalization_factor)
       obj = obj@RigidBodyContactWrench(robot,finger_idx,finger_grasp_pt);
       if(~isnumeric(grasp_object_idx) || numel(grasp_object_idx) ~= 1)
         error('Drake:GraspFrictionConeWrench: grasp_object_idx should be a scalar');
@@ -45,15 +45,14 @@
       end
       FC_axis_norm = sqrt(sum(grasp_object_FC_axis.^2,1));
       obj.grasp_object_FC_axis = grasp_object_FC_axis./(bsxfun(@times,FC_axis_norm,ones(3,1)));
-      g = 9.81;
       if(nargin<7)
-        force_normalize_factor = obj.robot.getMass*g;
+        force_normalization_factor = obj.robot.getMass*norm(obj.robot.gravity);
       else
-        if(~isnumeric(force_normalize_factor) || numel(force_normalize_factor) ~= 1 || force_normalize_factor<=0)
-          error('Drake:RigidBodyConeWrench: force_normalize_factor should be a positive scalar');
+        if(~isnumeric(force_normalization_factor) || numel(force_normalization_factor) ~= 1 || force_normalization_factor<=0)
+          error('Drake:RigidBodyConeWrench: force_normalization_factor should be a positive scalar');
         end
       end
-      obj.force_normalize_factor = force_normalize_factor;
+      obj.force_normalization_factor = force_normalization_factor;
       obj.cos_theta_square = 1./(obj.FC_mu.^2+ones(1,obj.num_pts));
       obj.num_wrench_constraint = 2*obj.num_pts;
       obj.num_pt_F = 3;
@@ -106,7 +105,7 @@
     end
 
     function A = force(obj)
-      A = obj.force_normalize_factor*speye(3*obj.num_pts);
+      A = obj.force_normalization_factor*speye(3*obj.num_pts);
     end 
 
     function [pos,J] = contactPosition(obj,kinsol)

diff --git a/solvers/trajectoryOptimization/SimpleDynamicsFullKinematicsPlanner.m b/solvers/trajectoryOptimization/SimpleDynamicsFullKinematicsPlanner.m
@@ -36,6 +36,10 @@
 
   methods
     function obj = SimpleDynamicsFullKinematicsPlanner(plant,robot,N,tf_range,Q_contact_force,contact_wrench_struct,options)
+      % @param plant   This is the plant going to be used in
+      % DirectTRajectoryOptimization, it can be different from robot since
+      % we want to use simple dynamics here. Refer to
+      % DirectTrajectoryOptimization for more details.
       % @param robot   A RigidBodyManipulator or a TimeSteppingRigidBodyManipulator
       % @param N   The number of knot points
       % @param tf_range  A double. The bounds on the total time

diff --git a/systems/plants/joints/RollPitchYawFloatingJoint.cpp b/systems/plants/joints/RollPitchYawFloatingJoint.cpp
@@ -30,7 +30,7 @@ void RollPitchYawFloatingJoint::motionSubspace(double* const q, MotionSubspaceTy
 {
   motion_subspace.resize(TWIST_SIZE, getNumVelocities());
   Map<Vector3d> rpy(&q[3]);
-	Matrix<double,SPACE_DIMENSION,RPY_SIZE> E;
+  Matrix<double,SPACE_DIMENSION,RPY_SIZE> E;
   rpydot2angularvelMatrix(rpy,E);
   auto R = rpy2rotmat(rpy);
   motion_subspace.block<3, 3>(0, 0).setZero();

diff --git a/util/drakeGeometryUtil.cpp b/util/drakeGeometryUtil.cpp
@@ -648,16 +648,16 @@ void rpydot2angularvelMatrix(const Eigen::MatrixBase<DerivedRPY>& rpy,
   typedef typename DerivedRPY::Scalar Scalar;
   Scalar p = rpy(1);
   Scalar y = rpy(2);
-	Scalar sp = sin(p);
-	Scalar cp = cos(p);
-	Scalar sy = sin(y);
-	Scalar cy = cos(y);
+  Scalar sp = sin(p);
+  Scalar cp = cos(p);
+  Scalar sy = sin(y);
+  Scalar cy = cos(y);
 
   using namespace std;
 	E << cp*cy, -sy, 0.0, cp*sy, cy, 0.0, -sp, 0.0, 1.0;
 	if(dE)
 	{
-		(*dE)<< 0.0, -sp*cy, -cp*sy, 0.0, -sp*sy, cp*cy, 0.0, -cp, 0.0, 0.0, 0.0, -cy, 0.0, 0.0, -sy, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+	  (*dE)<< 0.0, -sp*cy, -cp*sy, 0.0, -sp*sy, cp*cy, 0.0, -cp, 0.0, 0.0, 0.0, -cy, 0.0, 0.0, -sy, 0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
 	}
 }
 

diff --git a/util/test/testGeometryConversionFunctionsmex.cpp b/util/test/testGeometryConversionFunctionsmex.cpp
@@ -26,8 +26,8 @@ void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
 
   angularvel2quatdotMatrix(q, omega2qd, &domega2qd);
   angularvel2rpydotMatrix(rpy, omega2rpyd, &domega2rpyd, &ddomega2rpyd);
-	Matrix<double, SPACE_DIMENSION, RPY_SIZE> rpyd2omega;
-	typename Gradient<Matrix<double,SPACE_DIMENSION,RPY_SIZE>,RPY_SIZE,1>::type drpyd2omega;
+  Matrix<double, SPACE_DIMENSION, RPY_SIZE> rpyd2omega;
+  typename Gradient<Matrix<double,SPACE_DIMENSION,RPY_SIZE>,RPY_SIZE,1>::type drpyd2omega;
   rpydot2angularvelMatrix(rpy,rpyd2omega,&drpyd2omega);
   quatdot2angularvelMatrix(q, qd2omega, &dqd2omega);
   auto R = quat2rotmat(q);