55namespace dairlib {
66namespace systems {
77
8+ using drake::BasicVector;
89using drake::multibody::JointActuator;
910using drake::multibody::MultibodyPlant;
1011using drake::systems::kUseDefaultName ;
1112
12- GearedMotor::GearedMotor (const MultibodyPlant<double >& plant,
13- const std::unordered_map<std::string, double >& max_motor_speeds)
13+ GearedMotor::GearedMotor (
14+ const MultibodyPlant<double >& plant,
15+ const std::unordered_map<std::string, double >& max_motor_speeds)
1416 : n_q(plant.num_positions()),
1517 n_v (plant.num_velocities()),
1618 n_u(plant.num_actuators()),
@@ -19,11 +21,12 @@ GearedMotor::GearedMotor(const MultibodyPlant<double>& plant,
1921 const JointActuator<double >& joint_actuator =
2022 plant.get_joint_actuator (drake::multibody::JointActuatorIndex (i));
2123 actuator_gear_ratios.push_back (joint_actuator.default_gear_ratio ());
22- actuator_ranges.push_back (joint_actuator.effort_limit () / joint_actuator.default_gear_ratio ());
24+ actuator_ranges.push_back (joint_actuator.effort_limit () /
25+ joint_actuator.default_gear_ratio ());
2326 actuator_max_speeds.push_back (max_motor_speeds.at (joint_actuator.name ()));
2427 }
25- systems:: BasicVector<double > input (plant.num_actuators ());
26- systems:: BasicVector<double > output (plant.num_actuators ());
28+ BasicVector<double > input (plant.num_actuators ());
29+ BasicVector<double > output (plant.num_actuators ());
2730
2831 command_input_port_ =
2932 this ->DeclareVectorInputPort (" u_cmd" , input).get_index ();
@@ -36,11 +39,10 @@ GearedMotor::GearedMotor(const MultibodyPlant<double>& plant,
3639
3740void GearedMotor::CalcTorqueOutput (
3841 const drake::systems::Context<double >& context,
39- systems::BasicVector<double >* output) const {
40- const systems::BasicVector<double >& u =
41- *this ->template EvalVectorInput <BasicVector>(context,
42- command_input_port_);
43- const systems::BasicVector<double >& x =
42+ BasicVector<double >* output) const {
43+ const BasicVector<double >& u = *this ->template EvalVectorInput <BasicVector>(
44+ context, command_input_port_);
45+ const BasicVector<double >& x =
4446 *this ->template EvalVectorInput <BasicVector>(context, state_input_port_);
4547 Eigen::VectorXd actuator_velocities = B_.transpose () * x.value ().tail (n_v);
4648 Eigen::VectorXd tau = Eigen::VectorXd::Zero (n_u);
0 commit comments