search

DENSORobot / denso_cobotta_ros

denso_cobotta_ros’s documentation
https://densorobot.github.io/docs/denso_cobotta_ros/
GNU General Public License v2.0
13 stars 19 forks source link

Modifying joint_limits.yaml #14

Closed oishimilk closed 3 years ago

oishimilk commented 3 years ago

(updated: fix copy-and-paste failure of the script at 2020/07/27 Sun. 02:06 JST.)

Hi, thank you for COBOTTA OSS repository. I'm sorry for my poor English; please let me know if you find any unclear points.

I have a question about denso_cobotta_moveit_config/config/joint_limits.yaml.

According to COBOTTA User Manuals [Rev.2.073 (2019-03-01)] ID: 3702 (Daily Inspections), COBOTTA needs to move its all axes to prevent damage to the bearings.

I wrote a script shown below to move arms.

#!/usr/bin/env python
"""
This script moves axes and prevents COBOTTA from early wear-off of bearings.
See "NOTE" section of COBOTTA User Manuals [Rev.2.073 (2019-03-01)] ID: 3702 (Daily Inspections) for details.
"""

import rospy
import moveit_commander
import math

def main():
    """
    Main function.
    """
    rospy.init_node("daily_inspections", anonymous=True)
    scene = moveit_commander.PlanningSceneInterface()
    robot = moveit_commander.RobotCommander()
    arm = moveit_commander.MoveGroupCommander("arm")

    # Motion angle of each axis from COBOTTA User Manuals [Rev.2.073 (2019-03-01)] ID: 7260
    joints = {0: [-150, 150], 1: [-60, 100], 2: [18, 140], 3: [-170, 170], 4: [-95, 135], 5: [-170, 170]}

    # Number of iterations
    iterations = 10

    # initialization
    # Moves all axes to "neutral" angle.
    rospy.loginfo("Initializing all axes...")
    joint_goal = arm.get_current_joint_values()
    for j in range(len(joint_goal)):
        joint_goal[j] = math.radians((joints[j][0] + joints[j][1]) / 2)

        try:
            arm.go(joint_goal)
        except moveit_commander.exception.MoveItCommanderException as ex:
            print(ex)
        arm.stop()
        arm.clear_pose_targets()

    # Moves each axis from min to max.
    for j in range(len(joints)):
        for i in range(iterations):
            rospy.loginfo("Moving axis %d (%d/%d)..." % (j, i, iterations))

            joint_goal = arm.get_current_joint_values()
            joint_goal[j] = math.radians(joints[j][i % 2])

            multiplier = 1.0

            while multiplier > 1e-2 and not rospy.is_shutdown():
                try:
                    success = arm.go(joint_goal)
                except moveit_commander.exception.MoveItCommanderException as ex:
                    print("joint: %d: %s: %f [rad]" % (j, ex, joint_goal[j]))
                    success = False

                arm.stop()
                arm.clear_pose_targets()

                # Go to the next iteration.
                if success:
                    break

                # If the motion planning fails (e.g. collision with the ground), reduce the range of the angle.
                multiplier *= 0.9
                joint_goal[j] = math.radians((joints[j][0] + joints[j][1]) / 2) + joint_goal[j] * multiplier

        # Moves the axis to "neutral" angle.
        rospy.loginfo("Resetting axis %d..." % j)
        joint_goal[j] = math.radians((joints[j][0] + joints[j][1]) / 2)
        try:
            arm.go(joint_goal)
        except moveit_commander.exception.MoveItCommanderException as ex:
            print(ex)
        arm.stop()
        arm.clear_pose_targets()

    rospy.loginfo("All done.")

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        exit()

When I execute the script, I get ERROR messages like:

...
[ERROR] [1595494556.981926028]: /cobotta/denso_cobotta_driver: [Lv4:844A5001/00000000] <safety> J1 speed limit over
[ERROR] [1595494556.991894407]: /cobotta/denso_cobotta_driver: [Lv4:844A5001/00000000] <safety> J1 speed limit over
[ INFO] [1595494557.001881770]: /cobotta/denso_cobotta_driver: [Lv0:0F200202/00000000] <robot#0> Motor is OFF
[ERROR] [1595494557.011896937]: /cobotta/denso_cobotta_driver: [Lv4:84405501/00000601] <robot#0> J1:Bus voltage failure
[ERROR] [1595494557.013605907]: /cobotta/denso_cobotta_driver: [Lv4:84405502/00000601] <robot#0> J2:Bus voltage failure
[ERROR] [1595494557.013791882]: /cobotta/denso_cobotta_driver: [Lv4:84405503/00000601] <robot#0> J3:Bus voltage failure
[ERROR] [1595494557.015826998]: /cobotta/denso_cobotta_driver: [Lv4:84405504/00000601] <robot#0> J4:Bus voltage failure
[ERROR] [1595494557.017741371]: /cobotta/denso_cobotta_driver: [Lv4:84405505/00000601] <robot#0> J5:Bus voltage failure
[ERROR] [1595494557.018185885]: /cobotta/denso_cobotta_driver: [Lv4:84405506/00000601] <robot#0> J6:Bus voltage failure
[ERROR] [1595494557.019415086]: /cobotta/denso_cobotta_driver: [Lv4:84405501/00000601] <robot#1> J1:Bus voltage failure
...

Driver version:

[ INFO] [1595486395.517196523]: /cobotta/denso_cobotta_driver: Driver: Linux driver: 1.2.0-0 (Requirements: 1.2.N-M)
[ INFO] [1595486395.517669691]: /cobotta/denso_cobotta_driver: Driver: FPGA MAIN: cbmtA09G,02/07/2019
[ INFO] [1595486395.518352179]: /cobotta/denso_cobotta_driver: Driver: FPGA MAIN backup: cbmtF09G,02/07/2019
[ INFO] [1595486395.519346084]: /cobotta/denso_cobotta_driver: Driver: ServoMCU: 1010-1010-1010-1010-1010-1010-0000-0000-1010
[ INFO] [1595486395.520386826]: /cobotta/denso_cobotta_driver: Driver: SafetyMCU: 1003,1003-1003,1003-1003,1003-1003,1003-1003,1003-1003,1003-0000,0000-0000,0000-1003,1003

denso_cobotta_ros package revision: 27e185522ae241b9361dd4180d519fb46401a3f2 (Oct 29, 2019) branch: master $ uname -a: Linux tlab-cobotta 4.15.0-54-generic #58~16.04.1-Ubuntu SMP Mon Jun 24 13:21:41 UTC 2019 x86_64 x86_64 x86_64 GNU/Linux

I found that modifying max_velocity of denso_cobotta_moveit_config/config/joint_limits.yaml solves the error (and some axes seem to allow higher value).

axis (joint) current value proposed value
1 0.4031710572 0.36
2 0.3921754829 0.35
3 0.725707903 0.925
4 0.7391469382 0.9375
5 0.7391469382 1
6 1.1093312726 1.5

This is my joint_limits.yaml. I executed the script 5 times (50 iterations) with this settings and didn't see the error messages.

And my question is: Is modifying it unsafe for the robot? Your help would be much appreciated.

DensoWaveRobot commented 3 years ago

@oishimilk I'm very sorry for my reply too late. If you see this, please use the latest version denso_cobotta_ros. https://github.com/DENSORobot/denso_cobotta_ros/tree/melodic-devel

Latest version is modefied planning_adapter so it is far likely not to occur speed limit error.

oishimilk commented 3 years ago

Hi @DensoWaveRobot, Thank you very much for your answer. I'll upgrade the denso_cobotta_ros package of my COBOTTA.