I am trying to calculate the result of the inverse dynamic for a plant, however, I have noticed that the returned vector doesn't consider the gravity of the links.
For example, for a simple 1 link robot with a mass of 2 Kg that is positioned at the pi/4 angle, the following inverse dynamics calculation returns 0 without considering the weight of the link.
auto multibody_forces = MultibodyForces<double>(plant);
auto tau = plant.CalcInverseDynamics(plant_context, VectorXd::Zero(1), multibody_forces);
// tau will be 0
Is this a bug or a mis-documentation for CalcInverseDynamics?
MultibodyPlant::CalcInverseDynamics() does not include any forces. It only performs the documented operation. That is, given a set of external forces, it performs the computation:
tau = M(q)v̇ + C(q, v)v - tau_app - ∑ J_WBᵀ(q) Fapp_Bo_W
where the applied external are supplied with external_forces.
If you would like to only include gravity in that computation, you'd need to add it to external_forces. You can do that with:
// Generalized forces due to gravity depend on the configuration and
// therefore we must set positions "before" we make any computation.
VectorXd postions = ... // Code to set vector of positions.
plant.SetPositions(&context, positions);
// With the configuration set, we can now compute generalized forces.
MultibodyForces<double> external_forces(plant);
external_forces.generalized_forces() = plant.CalcGravityGeneralizedForces(context);
// Finally followed by the inverse dynamics computation.
const VectorXd tau_id = plant.CalcInverseDynamics(context, known_vdot, external_forces);
Here is a test that confirms that MultibodyPlant::CalcInverseDynamics does include the gravity torques.
Is it possible that you are creating the plant from a urdf/sdf that might not be putting the mass where you expect? For instance, if the gravity of the link was at the joint instead of along the arm, you might see this?
Related
I'm trying to use Drake's inverse dyanmics controller on an arm with a floating base, and based on this discussion it seems like the most straightforward way to go about this is to use two separate plants since the controller only supports fully actuated systems.
Following Python bindings error when adding two plants to a scene graph in pyDrake, I attempted to create two plants using the following code:
def register_plant_with_scene_graph(scene_graph, plant):
plant.RegsterAsSourceForSceneGraph(scene_graph)
builder.Connect(
plant.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(plant.get_source_id()),
)
builder.Connect(
scene_graph.get_query_output_port(),
plant.get_geometry_query_input_port(),
)
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
plant_1 = builder.AddSystem(MultibodyPlant(time_step=0.0))
register_plant_with_scene_graph(scene_graph, plant_1)
plant_2 = builder.AddSystem(MultibodyPlant(time_step=0.0))
register_plant_with_scene_graph(scene_graph, plant_2)
which produced the error
AttributeError: 'MultibodyPlant_[float]' object has no attribute 'RegsterAsSourceForSceneGraph'
Which seems odd because according to the documentation, the function should exist.
Is this function available in the python bindings for drake? Also, more broadly, is this the correct way to approach using the inverse dynamics controller on a free-floating manipulator?
Inverse dynamics takes desired positions, velocities, and accelerations and computes the required torques. If your robot has a floating base, then you cannot accept arbitrary acceleration commands. For instance the total center of mass of your robot will be falling according to gravity; any acceleration that does not satisfy this requirement will not have a feasible solution to the inverse dynamics. I think there must be something more that we need to understand about your problem formulation.
Often when people ask this question, they are thinking of a robot that is relying on contact forces in addition to generalized force/torques in order to achieve the requested accelerations. In that case, the problem needs to include those contact forces as decision variables, too. Since contact forces have unilateral constraints (e.g. feet cannot pull on the ground), and friction cone constraints, this inverse dynamics problem is almost always formulated as a quadratic program. For instance, as in this paper. We don't currently provide that QP formulation in Drake, but it is not hard to write it against the MathematicalProgram interface. And we do have some older code that was removed from Drake (since it wasn't actively developed) that we can point you to if it helps.
I'm using Tensorflow Federated (TFF) to train with differential privacy. Currently I am creating a Tensorflow Privacy NormalizedQuery and then passing it into a TFF DifferentiallyPrivateFactory to create an AggregationProcess:
_weights_type = tff.learning.framework.weights_type_from_model(placeholder_model)
query = tensorflow_privacy.GaussianSumQuery(l2_norm_clip=10.0, stddev=0.1)
query = tensorflow_privacy.NormalizedQuery(query, 20)
agg_proc = tff.aggregators.DifferentiallyPrivateFactory(query)
agg_proc = agg_proc.create(_weights_type.trainable)
After broadcasting the server state to clients I run a client update function and then use the AggregationProcess like this:
agg_output = agg_proc.next(
server_state.delta_aggregate_state,
client_outputs.weights_delta)
This works great, however I want to experiment with changing the l2_norm_clip and stddev several times during training (making clipping bigger and smaller at various training rounds) but it seems I can only set these parameters when I create the AggregationProcess.
Is is possible to change these parameters during training somehow?
I can think of two ways to do what you want: the easy way and the right way.
The right way is to make a new type of DPQuery that keeps track of the training round in its global state and adjusts the clip and stddev the way you want in its get_noised_result function. Then you can pass this new DPQuery to tff.aggregators.DifferentiallyPrivateFactory and use it like normal.
The easy way is to directly hack into the server_state.delta_aggregate_state. Somewhere in there you should find the global state of the DPQuery which should contain the l2_norm_clip and stddev which you can manipulate directly between rounds. This approach may be brittle because the aggregator state and the DPQuery state representations may be subject to change.
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").
I'm running tensorflow 2.1 and tensorflow_probability 0.9. I have fit a Structural Time Series Model with a seasonal component.
I wish to implement an Integrated Random walk in order to smooth the trend component, as per Time Series Analysis by State Space Methods: Second Edition, Durbin & Koopman. The integrated random walk is achieved by setting the level component variance to equal 0.
Is implementing this constraint possible in Tensorflow Probability?
Further to this in Durbin & Koopman, higher order random walks are discussed. Could this be implemented?
Thanks in advance for your time.
If I understand correctly, an integrated random walk is just the special case of LocalLinearTrend in which the level simply integrates the randomly-evolving slope component (ie it has no independent source of variation). You could patch this in by subclassing LocalLinearTrend and fixing level_scale = 0. in the models it builds:
class IntegratedRandomWalk(sts.LocalLinearTrend):
def __init__(self,
slope_scale_prior=None,
initial_slope_prior=None,
observed_time_series=None,
name=None):
super(IntegratedRandomWalk, self).__init__(
slope_scale_prior=slope_scale_prior,
initial_slope_prior=initial_slope_prior,
observed_time_series=observed_time_series,
name=None)
# Remove 'level_scale' parameter from the model.
del self._parameters[0]
def _make_state_space_model(self,
num_timesteps,
param_map,
initial_state_prior=None,
initial_step=0):
# Fix `level_scale` to zero, so that the level
# cannot change except by integrating the
# slope.
param_map['level_scale'] = 0.
return super(IntegratedRandomWalk, self)._make_state_space_model(
num_timesteps=num_timesteps,
param_map=param_map,
initial_state_prior=initial_state_prior,
initial_step=initial_step)
(it would be mathematically equivalent to build a LocalLinearTrend with level_scale_prior concentrated at zero, but that constraint makes inference difficult, so it's generally better to just ignore or remove the parameter entirely, as I did here).
By higher-order random walks, do you mean autoregressive models? If so, sts.Autoregressive might be relevant.