forked from Oeser/XBotInterface
-
Notifications
You must be signed in to change notification settings - Fork 0
/
interactive_requirements.txt
31 lines (20 loc) · 1.3 KB
/
interactive_requirements.txt
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
_robInteractive Requirements
XBotInterface& r = *XBotInterfacePtr::getRobot(path_to_cfg);
DynamicModel& m = *r.getModel("RBDL"); //default 2 arg : non sync, lib type from cfg
r.sense() // Updates all kinematic chains with data from /joint_states, IMUs, FTs, ...
m.sync(r) // Align model with actual robot
Eigen::Affine3d m.getTransform(r.floatingBaseName(), r.leg(0).tipLinkName()); // ros tf abstraction, notice that each kinematic chains has a getter for link names/joint names/etc (this is useful for specifying frame names from their semantics (I want to avoid hard-coded names!)
for(int i = 0; i<r.legs(); i++){
Eigen::VectorXd q_leg;
r.leg(i).getJointPosition(q_leg);
r.leg(i).setJointPositionReference(q_leg*2); // command twice the current position to quickly break the robot
}
r.write();// Send ref to low level controller
Eigen::VectorXd q = m.q(), q_dot; // get model joint positions
// Do some computations....
m.q() += q_dot*deltaT; // Add a vector to model configuration
r.setPosition(m) // Set a reference joint position to the whole actual robot from the model
r.write(); // Send it to low level controller
Eigen::Vector6d wrench;
r.ft.leg(0).getWrench(wrench); // Access FT sensor from its semantics, get measured wrench
r.ft("ft_leg_1").getWrench(wrench); // Access by name (hard-coded names :( )