search

Kinovarobotics / Kinova-kortex2_Gen3_G3L

Code examples and API documentation for KINOVA® KORTEX™ robotic arms
https://www.kinovarobotics.com/
Other
117 stars 87 forks source link

Manipulate Speeds using AngularWaypoint (Python) #133

Closed JohannaPrinz closed 1 year ago

JohannaPrinz commented 1 year ago

Description

I'm trying to implement a routine with different speeds, but I don't know how to set the speed correctly. By now the robot is moving with the same speed no matter with speeds I define:

ex = ProxemicsJointRoutines() ex.example_send_joint_angles([10.0, -10.0, 0.0, -10.0, 0.0, -5.0], [1.0,1.0,1.0,1.0,1.0,1.0]) ex.example_send_joint_angles([-10.0, 10.0, 0.0, 10.0, 0.0, 5.0], [6.0,6.0,6.0,6.0,6.0,6.0]) i'm unsing the code out of the following example: ros_kortex/kortex_examples/src/full_arm/example_full_arm_movement.py

Version

KortexAPI : ros_kortex (noetic-devel)

Kortex Device : Kinova DOF 6

Code example

def example_send_joint_angles(self, angles = [0.0 for i in range(6)], speeds=[float(3.0) for i in range(6)]) :

        self.last_action_notif_type = None

        # joint_states = rospy.wait_for_message("/" + self.robot_name + "/joint_states", )
        feedback = rospy.wait_for_message("/" + self.robot_name + "/base_feedback", BaseCyclic_Feedback)

        req = ExecuteActionRequest()

        trajectory = WaypointList()
        waypoint = Waypoint()
        angularWaypoint = AngularWaypoint()

        angularWaypoint.maximum_velocities.clear()

        # Angles to send the arm to vertical position (all zeros)
        for actuator, angle in zip(feedback.actuators, angles):
            angularWaypoint.angles.append(actuator.position + angle)

        # Each AngularWaypoint needs a duration and the global duration (from WaypointList) is disregarded. 
        # If you put something too small (for either global duration or AngularWaypoint duration), the trajectory will be rejected.
        angular_duration = 0
        angularWaypoint.duration = angular_duration

        angularWaypoint.maximum_velocities = speeds

        # Initialize Waypoint and WaypointList
        waypoint.oneof_type_of_waypoint.angular_waypoint.append(angularWaypoint)
        trajectory.duration = 0
        trajectory.use_optimal_blending = False
        trajectory.waypoints.append(waypoint)

        print(waypoint)

        try:
            res = self.validate_waypoint_list(trajectory)
        except rospy.ServiceException:
            rospy.logerr("Failed to call ValidateWaypointList")
            return False

        error_number = len(res.output.trajectory_error_report.trajectory_error_elements)
        MAX_ANGULAR_DURATION = 30

        while (error_number >= 1 and angular_duration != MAX_ANGULAR_DURATION) :
            angular_duration += 1
            trajectory.waypoints[0].oneof_type_of_waypoint.angular_waypoint[0].duration = angular_duration

            try:
                res = self.validate_waypoint_list(trajectory)
                print("RES: ",res)
                if (len(res.output.trajectory_error_report.trajectory_error_elements) != 0):
                    print(res.output.trajectory_error_report.trajectory_error_elements)
            except rospy.ServiceException:
                rospy.logerr("Failed to call ValidateWaypointList")
                return False

            error_number = len(res.output.trajectory_error_report.trajectory_error_elements)

        if (angular_duration == MAX_ANGULAR_DURATION) :
            # It should be possible to reach position within 30s
            # WaypointList is invalid (other error than angularWaypoint duration)
            rospy.loginfo("WaypointList is invalid")
            return False

        req.input.oneof_action_parameters.execute_waypoint_list.append(trajectory)
        print("REQ: ", req)

        # Send the angles
        rospy.loginfo("Sending the robot vertical...")
        try:
            self.execute_action(req)
        except rospy.ServiceException:
            rospy.logerr("Failed to call ExecuteWaypointjectory")
            return False
        else:
            return self.wait_for_action_end_or_abort()

Expected behavior

I expected the robot to move slower, when I send a speed array with [1.0,1.0,1.0,1.0,1.0,1.0] and faster when I send a speed array with [5.0,5.0,5.0,5.0,5.0,5.0]

Would be very happy if anybody could help me with that =)

felixmaisonneuve commented 1 year ago

Hi @JohannaPrinz,

Do you know what are the joint velocities when your are moving the arm? Is it slower than 1.0 deg/s?

I can't quite remember, but I am not sure you can do this (you cannot allocate an array to a protobuf object, you need to append every value)

angularWaypoint.maximum_velocities = speeds

and I think you should do something like this instead

        for i in range(6):
            angularWaypoint.maximum_velocities.append(speeds[i])

Start with this, if it doesn't work I will try to test it on my side next week.

Best, Felix

frfournier commented 1 year ago

You can assign to a repeated field straight from a list using .extend See following example:

from kortex_api.autogen.messages.Base_pb2 import AngularWaypoint

speeds=[float(3.0) for i in range(6)]
# Can be replaced by [3.0] * 6 for brevity

angularWaypoint = AngularWaypoint()
angularWaypoint.maximum_velocities.extend(speeds)

# Fill with fake values
for i in range(6):
    angularWaypoint.angles.append(i)
angularWaypoint.duration = 0.0

print(angularWaypoint)
JohannaPrinz commented 1 year ago

Thanks for the fast answer. unfortunately none of these solution worked. The robot still moves with the same speed... The request I send looks like this: ` input: handle: identifier: 0 action_type: 0 permission: 0 name: '' application_data: '' oneof_action_parameters: send_twist_command: [] send_wrench_command: [] send_joint_speeds: [] reach_pose: [] reach_joint_angles: [] toggle_admittance_mode: [] snapshot: [] switch_control_mapping: [] navigate_joints: [] navigate_mappings: [] change_twist: [] change_joint_speeds: [] change_wrench: [] apply_emergency_stop: [] clear_faults: [] delay: [] execute_action: [] send_gripper_command: [] send_gpio_command: [] stop_action: [] play_pre_computed_trajectory: [] execute_sequence: [] execute_waypoint_list:

` So the angles and the maximum_velocities are there.

JohannaPrinz commented 1 year ago

Do I may have to dis-/enable something in the Kinova Web-application to make this work?

JohannaPrinz commented 1 year ago

I'm now trying to send the speeds before appending the waypoints in a separated way like this: so now I do:

if name == "main": ex = ProxemicsJointRoutines() ex.send_joint_speeds([1.0,1.0,1.0,1.0,1.0,1.0]) ex.example_send_joint_angles([10.0, -10.0, 0.0, -10.0, 0.0, -5.0]) (s. code example above)

The new methode:

` def send_joint_speeds(self, speeds=[float(10.0) for i in range(6)]):

    self.last_action_notif_type = None

    req = ExecuteActionRequest()

    base_msg = Base_JointSpeeds()

    joint_speeds = []

    for jid in range(6):
        identfier = jid + 1
        joint_speed_msg = JointSpeed()
        joint_speed_msg.joint_identifier = identfier
        joint_speed_msg.value = speeds[jid]
        joint_speed_msg.duration = 0

        joint_speeds.append(joint_speed_msg)

    base_msg.joint_speeds = joint_speeds
    base_msg.duration = 0
    print(base_msg)

    req.input.oneof_action_parameters.send_joint_speeds.append(base_msg)

    # Send speeds
    rospy.loginfo("Sending the speeds...")
    try:
        self.execute_action(req)
    except rospy.ServiceException:
        rospy.logerr("Failed to call SendJointSpeedsCommandRequest")
        return False
    else:
        return self.wait_for_action_end_or_abort()

`

but I get the following output and error message:

joint_speeds:

[INFO] [1669030850.718962]: Sending the speeds... [ERROR] [1669030850.725003]: Failed to call SendJointSpeedsCommandRequest name: '' oneof_type_of_waypoint: angular_waypoint:

Is it even possible to do it this way, or do I have to do something else?

JohannaPrinz commented 1 year ago

Okay now it works and my Request looks like this - but the robot still moves with the same speed, weather the value of the joints is 1.0 or 6.0:

input: handle: identifier: 0 action_type: 0 permission: 0 name: '' application_data: '' oneof_action_parameters: send_twist_command: [] send_wrench_command: [] send_joint_speeds:

felixmaisonneuve commented 1 year ago

After some research on this, velocity constraints do not work on Angular movement, only durations. The arm will then complete the trajectory in the exact time of the duration set.

Each joint moves at a different velocity based on the movement it has to do and the duration available. For example, if your joint 2 need to go from 150deg to 170deg and your duration is 2s, it will move at an average of 10deg/s. This does not take into account the acceleration of the joint, which cannot be ignored in a real scenario.

I hope this helps, Felix

JohannaPrinz commented 1 year ago

Thanks a lot for your research! So if i use the cartesian waypoint action i could use the speed?!

felixmaisonneuve commented 1 year ago

Exactly, you can change the arm's linear and angular velocity for cartesian movements

felixmaisonneuve commented 1 year ago

Hi @JohannaPrinz,

I am closing this issue since I think I answered you. If you still have any question, feel free to re-open the issue or open a new one

Best Regards, Felix