RIVeR-Lab / epos_hardware

Other
12 stars 39 forks source link

2 Motor Velocity #13

Open Milliau opened 7 years ago

Milliau commented 7 years ago

How is it possible to control more than 1 motor, I have initialized both motor in 1 launch and this works, I also can send a float to the velocity_controller/command and the last initalized motor drives. But how can i send a velocity to the other motor. Is it possible to get 2 velocity_controller/command and how i will get them

Thanks for help

grafoteka commented 7 years ago

Hi @Milliau, I'm trying the same as you.

If you launch rosrun epos_hardware epos_hardware_node motor1 motor2 motor3 is not valid. It says that you must specify an actuator name, serial number and operation mode for each motor. But, if you read the wiki it only says to write: $ rosrun epos_hardware epos_hardware_node motor_name1 motor_name2 motor_name3

Milliau commented 7 years ago

@grafoteka yes if i want to start the programm with epos_node i get the same Problem, i just can start it with roslaunch example.launch but then i just have 1 Motor, i tryed to get both motors in one example file and it work to initalize but i can't drive both

I think the proböle is that if you just start the epos_hardware node it search with the motor_name for the information and can't find them but I'm not sure

grafoteka commented 7 years ago

@Milliau yes, I can also drive any motor modifying the node ID. I don't know if it would be necessary to change the yaml file or some configuration

velinddimitrov commented 7 years ago

@Milliau @grafoteka

Here is an example controlling multiple motors: https://github.com/RIVeR-Lab/walrus/tree/master/walrus_base_hw/config

Milliau commented 7 years ago

@velinddimitrov thank you for the example i have a few questions i saw that you didn't use the joint_state_controller and the velocity_controller but you use walrus_pod_hw::WalrusPodHW robot(nh, pnh, "/dev/walrus_front_pod_controller", "/dev/walrus_back_pod_controller") i think POD Controlle ris a special controller, i have an logitech gamepad ti control is it possible to control with this? Can you give my an idea what i have to change? Thank you

velinddimitrov commented 7 years ago

That controller will probably not be particularly useful to you since Walrus was a robot with "pods" that had tracks on them and moved up and down. I don't know what format your robot has, but you may want to take a look at the Husky or Turtlebot controllers for examples of skid-steer robots. That'll probably match closer to what you are looking at. I sent you the info on Walrus so you could see the configuration files for talking to 2 motors. Once you can move both motors (by publishing to their respective topics for example), you can just follow the ros_control example to implement whatever higher level control modality you wish.

grafoteka commented 6 years ago

Hi @velinddimitrov I continue trying to move two motors, but I'm stuck. I have read the Walrus github, and from it I have modified the URDF file:

<?xml version="1.0"?>
<robot name="example">

  <!-- MOTOR 1 -->
  <link name="link11">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <link name="link12">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="test_joint" type="revolute">
    <parent link="link11"/>
    <child link="link12"/>
  </joint>
  <transmission name="test_joint_trans_1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="test_joint_1">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="test_joint_actuator_1">
      <mechanicalReduction>333.0</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- MOTOR 2 -->
    <link name="link21">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <link name="link22">
    <inertial>
      <mass value="1"/>
      <inertia ixx="1" iyy="1" izz="1" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="test_joint" type="revolute">
    <parent link="link21"/>
    <child link="link22"/>
  </joint>
  <transmission name="test_joint_trans_2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="test_joint_2">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="test_joint_actuator_2">
      <mechanicalReduction>1000</mechanicalReduction>
    </actuator>
  </transmission>

</robot>

Then create two config files: drive_epos.yaml and epos_motor.yaml as you have done. And finally modify the launch file:

<launch>

  <param name="robot_description" textfile="$(find epos_hardware)/launch/example_2.urdf" />

  <!--node name="epos_hardware" pkg="epos_hardware" type="epos_hardware_node">
    <rosparam command="load" file="$(find epos_hardware)/config/motores_2.yaml" output="screen"/>
  </node-->

  <node name="epos_hardware"
        pkg="epos_hardware"
        type="epos_hardware_node"
        args="'left_drive_actuator' 'right_drive_actuator'"
        output="screen">
    <remap from="epos_hardware/joint_states" to="joint_states" />
    <param name="controller_rate" value="10.0" type="double" />
    <rosparam command="load" file="$(find epos_hardware)/config/epos_motors_2.yaml" />
    <rosparam command="load" file="$(find epos_hardware)/config/drive_epos_motor.yaml" param="left_drive_actuator"/>
    <rosparam command="load" file="$(find epos_hardware)/config/drive_epos_motor.yaml" param="right_drive_actuator"/>
  </node>

  <!-- Control en velocidad -->
  <node name="controller_spawner" 
        pkg="controller_manager" 
        type="spawner" respawn="false"
        output="screen" 
        args="joint_state_controller_1 velocity_controller_1 joint_state_controller_2 velocity_controller_2"/>

    <param name="velocity_controller_1/type" value="velocity_controllers/JointVelocityController" />
    <param name="velocity_controller_1/joint" value="test_joint_1" />
    <param name="position_controller_1/type" value="position_controllers/JointPositionController" />
    <param name="position_controller_1/joint" value="test_joint_1" />
    <param name="joint_state_controller_1/type" value="joint_state_controller/JointStateController" />
    <param name="joint_state_controller_1/publish_rate" value="50" />

    <param name="velocity_controller_2/type" value="velocity_controllers/JointVelocityController" />
    <param name="velocity_controller_2/joint" value="test_joint_2" />
    <param name="position_controller_2/type" value="position_controllers/JointPositionController" />
    <param name="position_controller_2/joint" value="test_joint_2" />
    <param name="joint_state_controller_2/type" value="joint_state_controller/JointStateController" />
    <param name="joint_state_controller_2/publish_rate" value="50" />

</launch>

But when I run the launch file I have the next error:

Loading controller: joint_state_controller_1
[ERROR] [1511443555.370774901]: This controller requires a hardware interface of type 'hardware_interface::JointStateInterface'. Make sure this is registered in the hardware_interface::RobotHW class.
[ERROR] [1511443555.370798549]: Initializing controller 'joint_state_controller_1' failed
[ERROR] [1511443556.372057]: Failed to load joint_state_controller_1
[INFO] [1511443556.372337]: Loading controller: velocity_controller_1
[ERROR] [1511443556.391896751]: This controller requires a hardware interface of type 'hardware_interface::VelocityJointInterface'. Make sure this is registered in the hardware_interface::RobotHW class.
[ERROR] [1511443556.391918263]: Initializing controller 'velocity_controller_1' failed
[ERROR] [1511443557.393331]: Failed to load velocity_controller_1
[INFO] [1511443557.394261]: Loading controller: joint_state_controller_2
[ERROR] [1511443557.400715048]: This controller requires a hardware interface of type 'hardware_interface::JointStateInterface'. Make sure this is registered in the hardware_interface::RobotHW class.
[ERROR] [1511443557.400800050]: Initializing controller 'joint_state_controller_2' failed
[ERROR] [1511443558.402454]: Failed to load joint_state_controller_2
[INFO] [1511443558.403407]: Loading controller: velocity_controller_2
[ERROR] [1511443558.409620122]: This controller requires a hardware interface of type 'hardware_interface::VelocityJointInterface'. Make sure this is registered in the hardware_interface::RobotHW class.
[ERROR] [1511443558.409700413]: Initializing controller 'velocity_controller_2' failed
[ERROR] [1511443559.411225]: Failed to load velocity_controller_2
zhangstar3 commented 5 years ago

@grafoteka ,oh I have see this issue,I see that this package has some difficulties in controlling multiple motors. Is there any improvement in this situation? Now I'm going to use epos, Maxon motors to make a manipulator.Hope you can give some suggestion.thanks!