Explanation about ClusterJoints state

[roncapat]

Hi again, sorry for another "issue". This time is more about a clarification about clusters with a loop.

Taking as an example the four_bar.urdf included in this repo, I did the following:

  grbda::ModelState<double> model_state;
  for (const auto &cluster : urdf_model.clusters()) {
      grbda::JointState<> joint_state = cluster->joint_->randomJointState();
      model_state.push_back(joint_state);
  }

  urdf_model.setState(model_state);
  std::cout << model_state.size() << std::endl;

  for (int i = 0; i<model_state.size(); ++i) {
		std::cout << "Cluster: " << urdf_model.clusters()[i]->name_ << std::endl;
    auto joints = urdf_model.clusters()[i]->joint_->singleJoints();
    std::cout << "Joints:" ;
    for (const auto & j : joints) {
      std::cout << " " << j->name();
    }
    std::cout << std::endl;
    std::cout << "Pos: " << model_state[i].position.transpose() << std::endl
              << "Vel: " << model_state[i].velocity.transpose() << std::endl;
	}

obtaining:

Cluster: cluster-0
Joints: joint1 joint3 joint2
        Pos: -0.0866771 -0.0866771  0.0866771 
        Vel: 0.680375

I expected here to have only one independent position and one independent velocity, as all the remaining positions in the 4-bar linkages are dependent on the "input" joint.

What am I missing here? If I would like to animate the 4-bar linkage by setting something else than a random joint state vector, on which variable or through which interface should I act before calling forwardKinematics? Should I just edit the 1st element of the pos vector and disregard the other ones?

Moreover, I see that the parsed URDF+ loop is implemented as GenericJoint, whose randomJointState() relies on a rand+retry approach inside and outside the call to findRootsForPhi(). It seems to me that there is no way to impose the value of the independent variable to obtain coherent dependent ones.

[nicholasadr]

Hi @roncapat, thanks for the question.

The position and velocity output are as expected since FourBar::randomJointState() returns spanning configuration (3 elements) for the position and independent velocity (1 element).

Assuming that you have an initial spanning configuration and velocity for the four bar linkage, you can call the following lines to initialize the coordinate of the ClusterTreeModel before calling forwardKinematics:

DVec<double> q_spanning(3);
DVec<double> v_spanning(3);
q_spanning = // set your choice of spanning configuration
v_spanning = // set your choice of spanning velocity
JointCoordinate<double> joint_pos(q_spanning, true); // the second argument is true indicates it's a spanning configuration
JointCoordinate<double> joint_vel(v_spanning, true);
JointState<double> joint_state(joint_pos, joint_vel);
ModelState model_state;
model_state.push_back(joint_state);
cluster_tree.setState(model_state);

Once you have performed forward dynamics to obtain the independent acceleration (y), you can perhaps compute the spanning acceleration (qdd) and then perform integration to update the spanning configurations. Else, you can use the root finder to compute the dependent variables from the independent variable.

[roncapat]

Hi @nicholasadr, thank you for your kind feedback.
However, I think that a loop constraint in URDF is currently always encoded as a ClusterJoints::Generic and never as ClusterJoints::FourBar. Am I missing something?

generalized_rbda/src/Dynamics/ClusterTreeParsing.cpp

Lines 131 to 177 in 45fa7c4

	if (constraint_class_type == urdf::Constraint::LOOP)
	{
	// For each constraint, collect the captures that will be supplied to the lambda
	// function encoding the constraint
	std::vector<LoopConstraintCapture> constraint_captures;
	for (urdf::ConstraintSharedPtr constraint : cluster_constraints)
	{
	urdf::LoopConstraintSharedPtr loop_constraint =
	std::dynamic_pointer_cast<urdf::LoopConstraint>(constraint);
	std::vector<Body<SX>> predecessor_to_nca_subchain =
	subchainFromTo(loop_constraint->predecessor_link_name,
	loop_constraint->nearest_common_ancestor_name);
	std::vector<Body<SX>> successor_to_nca_subchain =
	subchainFromTo(loop_constraint->successor_link_name,
	loop_constraint->nearest_common_ancestor_name);
	LoopConstraintCapture capture;
	for (auto it = predecessor_to_nca_subchain.rbegin();
	it != predecessor_to_nca_subchain.rend(); ++it)
	{
	capture.nca_to_predecessor_subchain.push_back(*it);
	}
	for (auto it = successor_to_nca_subchain.rbegin();
	it != successor_to_nca_subchain.rend(); ++it)
	{
	capture.nca_to_successor_subchain.push_back(*it);
	}
	capture.predecessor_to_constraint_origin_transform = loop_constraint->predecessor_to_constraint_origin_transform;
	capture.successor_to_constraint_origin_transform = loop_constraint->successor_to_constraint_origin_transform;
	constraint_captures.push_back(capture);
	}
	// TODO(@MatthewChignoli): Would like to do some more sophisticated detection and specialization here. For example, if the jacobian of phi is constant, then we can use an explicit constraint. The best solution might just be codegen
	// Create constraints and add clusters
	std::string cluster_name = "cluster-" + std::to_string(cluster_nodes_.size());
	std::function<DVec<SX>(const JointCoordinate<SX> &)> phi;
	phi = implicitPositionConstraint(constraint_captures, joints_sx);
	using LoopConstraintType = LoopConstraint::GenericImplicit<Scalar>;
	std::shared_ptr<LoopConstraintType> generic_implicit;
	generic_implicit = std::make_shared<LoopConstraintType>(independent_coordinates, phi);
	appendRegisteredBodiesAsCluster<ClusterJoints::Generic<Scalar>>(
	cluster_name, bodies_in_current_cluster_,
	joints_in_current_cluster, generic_implicit);
	}

[roncapat]

BTW, I tried the <coupling> tag instead of the <loop> one to close a parallel 4-bar linkage in my URDF. Now I get, for the 4-bar cluster, a .position if size 1 and a .velocity of size 1 - which is a bit more intuitive. However, it is very unintuitive w.r.t. URDF official <mimic> tag, as I need to refer to links instead of joints. To close the 4-bar linkage, I had to set 1 coupling with ratio 1, and another one with a very big number as ratio (like 1e9). Do you have an example of URDF with a 4-bar linkage "closed" via coupling contraints?

[nicholasadr]

Hi @roncapat, my apology for the late reply.

1. There are two ways to build the ClusterTreeModel: URDF vs manual build. When building from URDF, the loop and coupling constraints are always encoded as ClusterJoints::Generic. When building manually, you can make use of the specialized joints such as the ClusterJoints::FourBar. Note that these specialized joints are derived from Generic. See this file for an example of building ClusterTreeModel manually that includes using the ClusterJoints::FourBar.

2. The coupling tag is used for systems in which the constraints can be expressed explicitly (a geared motor). The loop tag is used for systems with implicit constraints such as the four bar linkage.

3. For examples of URDF for four-bar linkage, see this file or this directory.

I hope the above answers were helpful!