diff --git a/src/SpringPlugin.cc b/src/SpringPlugin.cc index 3b753f4b31037dc6de7d896826e16786530ca828..300fc2d6a92ef3d61aa85c68257cffaa980daa44 100644 --- a/src/SpringPlugin.cc +++ b/src/SpringPlugin.cc @@ -29,21 +29,24 @@ SpringPlugin::SpringPlugin() } ///////////////////////////////////////////////// -void SpringPlugin::Load(physics::ModelPtr _model, - sdf::ElementPtr _sdf) +void SpringPlugin::Load(physics::ModelPtr lmodel, + sdf::ElementPtr lsdf) { - this->model = _model; + this->model = lmodel; // hardcoded params for this test - this->jointExplicitName = _sdf->Get<std::string>("joint_spring"); + if (!lsdf->HasElement("joint_spring")) + ROS_ERROR_NAMED("SpringPlugin","No field joint_spring for SpringPlugin"); + else + this->jointExplicitName = lsdf->Get<std::string>("joint_spring"); - this->kpExplicit = _sdf->Get<double>("kp"); + this->kpExplicit = lsdf->Get<double>("kp"); - this->kdExplicit = _sdf->Get<double>("kd"); + this->kdExplicit = lsdf->Get<double>("kd"); ROS_INFO_NAMED("SpringPlugin", "Loading joint : %s kp: %f kd: %f", - this->jointExplicitName, + this->jointExplicitName.c_str(), this->kpExplicit, this->kdExplicit); } diff --git a/urdf/leg/leg_passive.urdf.xacro b/urdf/leg/leg_passive.urdf.xacro index 2b3f3f439a2122a37add84235fd65dabe7843daa..a061f15ff68f13679433d48a16f2672008822375 100644 --- a/urdf/leg/leg_passive.urdf.xacro +++ b/urdf/leg/leg_passive.urdf.xacro @@ -69,6 +69,18 @@ <kd>0.0</kd> </plugin> </gazebo> + + <transmission name="leg_${prefix}_1_passive_trans"> + <type>transmission_interface/SimpleTransmission</type> + <actuator name="leg_${prefix}_1_motor" > + <mechanicalReduction>${reduction}</mechanicalReduction> + </actuator> + <joint name="leg_${prefix}_1_joint"> + <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> + <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface> + </joint> + </transmission> + <!-- Joint hip_x (mesh link) : child link hip_y -> parent link hip_z --> <!-- <link name="leg_${prefix}_2_link_passive"> -->