I have a modified version of the InverseKinematics class where I solve for multiple joints waypoints in one optimization. This is done by defining a q_ decision variable of size plant->num_positions() * num_waypoints, and slicing into q_ for the particular waypoint when defining costs/constraints. This way, q_.block(...).rows() == plant->num_positions(), satisfying the checks in the Binding constructor when calling AddConstraint().
This has worked well, but now I want to define a constraint that involves two of the waypoints. Given two waypoints q_fixed and q_free sliced from q_, I want to evaluate the forward kinematics for both q_fixed and q_free to obtain rotations R_fixed and R_free defined in some frame. My constraint would be bounding the error between these two rotations (ex: limiting end-effector rotational motion), and this would be applied on q_free, treating q_fixed as just a constant. When calling AddConstraint(constraint, q_) with my entire q_, I get the expected error:
abort: Failure at /opt/drake/include/drake/solvers/binding.h:30 in Binding(): condition 'c->num_vars() == v.rows() || c->num_vars() == Eigen::Dynamic' failed.
One possible workaround is to define the constraint with respect to q_free only and calling AddConstraint(constraint, q_free), but then passing some lambda function, symbolic::Formula, or Eigen::Ref<const VectorXDecisionVariable> into constraint. This way, constraint can access the current value of q_fixed in the optimization process. Is this possible?
Related
I want to simulate a 2D heat transfer process in the subsurface on a region which is infinite on the r-direction. So, as you know, the very basic way to model this is to draw a geometry that is very long in the r direction. I have done this, and the results that I obtain is correct as in this case, the results are matched with the analytical solution. As you know, there is a capability in Comsol called infinite element domain which serves the purpose to the problem mentioned above. In this case, we need to define a limited geometry on which we want to solve the PDE, and also need to draw a small domain acting as the Infinite Element Domain. However, in this case, the results are not correct because they are not matched with the analytical solution. Is there anything that I am missing to correctly use Infinite Element Domain in comsol?
Any help or comment would be appreciated.
Edit:
I edited the post to be more specific.
Please consider the following figure where a fluid with high temperature is being injected into a region with lower temperature:
https://i.stack.imgur.com/BQycC.png
The equation to solve is:
https://i.stack.imgur.com/qrZcK.png
With the following initial and boundary conditions (note that the upper and lower boundary condition is no-flux):
https://i.stack.imgur.com/l7pHo.png
We want to obtain the temperature profile over the length of rw<r<140 m (rw is very small and is equal to 0.005 m here) at different times. One way to model this numerically in Comsol is to draw a rectangle that is 2000 m in the r-direction, and get results only in the span of r [rw,140] m:
https://i.stack.imgur.com/BKCOi.png
The results of this case is fine, because they are well-matched with the analytical solution.
Another way to model this is to replace the above geometry with a bounded one that is [rw, 140] m in the r-direction and then augment it with an Infinite Element domain that is meshed mapped, as follows:
https://i.stack.imgur.com/m9ksm.png
Here, I have set the thickness of Infinite Element to 10 m in the r-direction. However, the results in this case are not matched with the analytical solution (or the above case where Infinite Element domain was not used). Is there anything that I am missing in Comsol? I have also changed some variables with regard to Infinite Element in Comsol such as physical width or distance, but I didn't see any changes in the results.
BTW, here are the results:
https://i.stack.imgur.com/cdaPH.png
While digitizing points using the Engauge digitizer software, the points become automatically sorted by the value of the horizontal variable. This is fine for functions where there is only one "y" for every "x". But I have curves the have multiple y-values for every x.
Is there some setting that I should be using?
Change the curve type from Function to Relation. Use Settings / Curve
Properties. Then choose 'Relation - Straight' or 'Relation - Smooth'. As
you have seen, the default is 'Function - Smooth'.
Response taken from: https://github.com/markummitchell/engauge-digitizer/issues/241
I am trying to get the Coriolis matrix for my robot (need the matrix explicitly for the controller) based on the following approach which I have found online:
plant_.CalcBiasTerm(*context, &Cv_);
auto jac = autoDiffToGradientMatrix(Cv_);
C = 0.5*jac.rightCols(n_v_);
where Cv_, plant_, context are AutoDiffXd and n_v_ is the number of generalized velocities. So basically I have a 62-joint robot loaded from URDF into drake which is a free body (floating base system). After finalizing the robot I am using the DiagramBuilder.Build() method and then the CreateDefaultContext() in order to get the context. Next, I am trying to set up the AutoDiff environment like this:
plant_autodiff = drake::systems::System<double>::ToAutoDiffXd(*multibody_plant);
context_autodiff = plant_autodiff->CreateDefaultContext();
context_autodiff->SetTimeStateAndParametersFrom(*diagram_context);
The code above is contained in an initialization setup code. In another method, which is called on update events, the following lines of code are written:
drake::AutoDiffVecXd c_auto_diff_ = drake::AutoDiffVecXd::Zero(62);
plant_autodiff->CalcBiasTerm(*context_autodiff, &c_auto_diff_);
MatrixXd jac = drake::math::autoDiffToGradientMatrix(c_auto_diff_);
auto C = 0.5*jac.rightCols(jac.size());
This setup compiles and runs, however the size of the jac matrix is 0, whereas I would expect 62x62. I am also extracting and then exposing the Coriolis vector, which is 62x1 and seems to be more or less correct. The c_auto_diff_ variable is 62x1 as well, but all the elements are 0.
I am clearly making a mistake, but I do not know where exactly.
Any help is appreciated,
Thank you all,
Robert
You are close. You need to tell the autodiff pipeline what you want to take the derivative with respect to. In this case, I believe you want
auto v = drake::math::initializeAutoDiff(Eigen::VectorXd::Zero(62))
plant_autodiff->SetVelocities(context_autodiff.get(), v);
By calling initializeAutoDiff, you are initializing the autodiff terms to the identity matrix, which is saying that you want to take the derivative with respect to v. Then you should get non-zero derivatives.
Btw - I normally would use
plant_autodiff = multibody_plant->ToAutoDiffXd();
but I guess what you have must work, too!
I'm creating a system of bodies with radially expanding bodies connected with PrismaticJoints, and finding that, although I initialized each joint with joint position limits, the joints pass these limits due to external forces like gravity easily. Here is a plot showing some joints' translations over time to show how they pass the lower and upper limits at 3.5 and 4.2:
What am I missing? My call to create the joints looks like this:
const multibody::Joint<double>& joint = plant_->AddJoint<drake::multibody::PrismaticJoint>(
shpere_name + "_joint",
center_body, std::nullopt,
connect_body, std::nullopt,
unitVlist()[j], r_low, r_upp, 0);
where
*_body are bodies,
unitVlist() returns a list of unit vectors to pull from,
r_low and r_upp are doubles corresponding to the lower and upper limits.
Currently joint limits in Drake are only enforced by the discrete solver, that is, what you get if you supply a time step in the MultibodyPlant constructor. Our continuous integrators don't see the limits yet. We are aware of that but I couldn't actually find a GitHub issue complaining about it -- would you mind filing one? You can do it here (select "New Issue").
In this repo, #Gizatt uses the following command to assemble collision constraints for the kuka iiwa:
ik.MinDistanceConstraint(tree, collision_tol, list(), set())
Here, what do list() and set() signify. Both seem to be empty here.
Let's just say I have an item (item 1) that consists of 6 bodies (within one urdf) and another object (item 2) in my RigidBodyTree that consists of one body (within a separate urdf) and I only want to check for collisions between any of the 6 bodies that make up item 1 and item 2. Is there a way to set this function so that it doesn't check for collisions within the all the bodies in item 1 but only for collisions between item 1 and item 2?
Finally, I currently have the following error when I use this function:
[2018-11-14 19:39:20.812] [console] [warning] Attempting to compute distance between two collision elements, at least one of which is non-convex.
I took #gizatt's advice and converted the meshes of each link within item 1 to convex hulls using meshlab and when I look at each mesh using the visualizer, they all appear to be convex to me. However I still get this error. Is there any other reason this error would pop up?
The documentation for that method is here:
https://drake.mit.edu/pydrake/pydrake.solvers.ik.html?highlight=mindistanceconstraint#pydrake.solvers.ik.MinDistanceConstraint
The last two arguments that greg used are about the "active" bodies. This will help you if you want to ignore some bodies entirely from the collision computation.
If you want to ignore some collision pairs, then use collision filter groups:
https://drake.mit.edu/pydrake/pydrake.multibody.rigid_body_tree.html?highlight=collision%20filter#pydrake.multibody.rigid_body_tree.RigidBodyTree.DefineCollisionFilterGroup
Our RBT/Bullet wrapper assumes every mesh is convex EXCEPT static/anchored geometry. It seems likely that you are getting that warning because you are collision checking against the static/anchored geometry?
FWIW -- the documentation is MUCH more complete on multibodytree vs rigidbodytree, but for this particular query I think you're right to use RBT -- multibody is not quite there yet.
Here, what do list() and set() signify. Both seem to be empty here.
These arguments are historical artifacts and are almost never what you want to use. I would recommend leaving them empty as in the example you provided. The first restricts the constraint to consider only the bodies specified by active_bodies_idx. The second restricts the constraint to consider only the collision groups whose names are contained in active_group_names. Note that the "collision group" concept for the active_group_names argument is not the same as the "collision filter group" concept.
Is there a way to set this function so that it doesn't check
for collisions within the all the bodies in item 1 but only for
collisions between item 1 and item 2?
You were on the right track. You want to add a collision filter group that contains all of the bodies in item 1 and then set that collision group to ignore itself. The following code requires the AddCollisionFilterIgnoreTarget() binding added by PR #10085.
tree.DefineCollisionFilterGroup("1_filtergroup")
tree.AddCollisionFilterGroupMember("1_filtergroup", "base_link", 1_model_id)
# Repeat the previous line for all bodies in item 1.
tree.AddCollisionFilterIgnoreTarget("1_filtergroup", "1_filtergroup")
You can then create a constraint with the desired behavior by calling ik.MinDistanceConstraint(tree, collision_tol, list(), set()).
Finally, I currently have the following error when I use this
function: [2018-11-14 19:39:20.812] [console] [warning] Attempting to
compute distance between two collision elements, at least one of which
is non-convex. ... Is there any other reason this error
would pop up?
As #Russ Tedrake mentioned, all mesh collision elements attached to anchored (welded to the world) bodies are converted to non-convex mesh objects in the Bullet backend, regardless of the convexity of the original mesh. This is unfortunate, but will most likely not be addressed, as all RigidBodyTree-related code is nearing end-of-life. For IK purposes, you can work around this by attaching the mesh to a dummy body that is connected to the world by a prismatic or revolute joint whose upper and lower limits are both 0. That will force the backend to work with the convex-hull of the provide mesh. You'll then need to remove the element corresponding to the dummy joint from the configuration returned by IK.