search

Kinovarobotics / ros_kortex

ROS packages for KINOVA® KORTEX™ robotic arms
Other
153 stars 155 forks source link

Issues with SendJointSpeedsCommand (for joint velocity control) #145

Closed akeaveny closed 3 years ago

akeaveny commented 3 years ago

Hi,

I'm facing issues trying to send /my_gen3_lite/in/joint_velocity of type kortex_driver/Base_JointSpeeds in a python script.

I've verified that I can send joint_velocities first using a command line input such as

rostopic pub /my_gen3_lite/in/joint_velocity kortex_driver/Base_JointSpeeds \
"{joint_speeds: [{joint_identifier: 0, value: 1.0, duration: 1}, \
        {joint_identifier: 0, value: 1.0, duration: 1}, \
        {joint_identifier: 1, value: 1.0, duration: 1}, \
        {joint_identifier: 2, value: 1.0, duration: 1}, \
        {joint_identifier: 3, value: 1.0, duration: 1}, \
        {joint_identifier: 4, value: 1.0, duration: 1}, \
        {joint_identifier: 5, value: 1.0, duration: 1}], \
        duration: 2}"

So far I've tried the following insdie my script:

 def publish_env_arm_cmd(self, vel_cmd):
        """ publish 'vel_cmd' action to ROS Controller """
        base_msg = Base_JointSpeeds()

        joint_speeds = []
        for joint_idx in range(self.num_joints):
            msg = JointSpeed()
            msg.joint_identifier = joint_idx
            msg.value = vel_cmd[joint_idx]
            msg.duration = 0
            joint_speeds.append(msg)
        base_msg.joint_speeds = joint_speeds
        base_msg.duration = 0
        print(base_msg)

        self.arm_cmd_pub.publish(msg)
        rospy.sleep(self.control_period)

The output of my script is

joint_speeds: 
  - 
    joint_identifier: 0
    value: 0.6522441591498449
    duration: 0
  - 
    joint_identifier: 1
    value: -0.045807120396657186
    duration: 0
  - 
    joint_identifier: 2
    value: 0.6661473528266665
    duration: 0
  - 
    joint_identifier: 3
    value: 0.6468793605631347
    duration: 0
  - 
    joint_identifier: 4
    value: 0.23506380046924785
    duration: 0
  - 
    joint_identifier: 5
    value: 0.6740915690125258
    duration: 0
duration: 0

The error I continue to face with my script is:

Traceback (most recent call last):
  File "/home/akeaveny/robogym_gen3_ws/src/robo-gym-robot-servers/gen3_robot_server/scripts/robot_server.py", line 59, in <module>
    serve()
  File "/home/akeaveny/robogym_gen3_ws/src/robo-gym-robot-servers/gen3_robot_server/scripts/robot_server.py", line 41, in serve
    RobotServerServicer(real_robot=real_robot, gen3_model= gen3_model), server)
  File "/home/akeaveny/robogym_gen3_ws/src/robo-gym-robot-servers/gen3_robot_server/scripts/robot_server.py", line 10, in __init__
    self.rosbridge = Gen3RosBridge(real_robot= real_robot, gen3_model= gen3_model)
  File "/home/akeaveny/robogym_gen3_ws/src/robo-gym-robot-servers/gen3_robot_server/scripts/ros_bridge.py", line 88, in __init__
    self.publish_env_arm_cmd(vel_cmd)
  File "/home/akeaveny/robogym_gen3_ws/src/robo-gym-robot-servers/gen3_robot_server/scripts/ros_bridge.py", line 264, in publish_env_arm_cmd
    self.arm_cmd_pub.publish(msg)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 879, in publish
    data = args_kwds_to_message(self.data_class, args, kwds)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/msg.py", line 121, in args_kwds_to_message
    raise TypeError("expected [%s] but got [%s]"%(data_class._slot_types[0], arg._type))
TypeError: expected [kortex_driver/JointSpeed[]] but got [kortex_driver/JointSpeed]

Any help would be appreciated!

Thanks, Aidan

akeaveny commented 3 years ago

Realized I should've had self.arm_cmd_pub.publish(base_msg)!