How to Set and Enforce Joint Limits?

Manipulators of various configurations
Post Reply
User avatar
MarkJennings
Posts: 8
Joined: Fri Jul 17, 2020 10:22 am
Contact:

How to Set and Enforce Joint Limits?

Post by MarkJennings » Thu Aug 06, 2020 7:17 pm

This is a bit of a follow-up from my previous post, but I thought this was enough of a new topic that it might warrant another thread. I need to keep motors within +-pi so that the cables routed through them don't twist too much. I believe this is set in hardware via Scope, but my controller (in this case, MoveIt) has no knowledge of this. My first approach to fix this was adding min_position and max_position constraints to each joint in the moveit_config joint_angles.yaml file. However, this doesn't work because MoveIt considers the joint to be continuous. I get the following error:

Code: Select all

Error: Cannot specify position limits for continuous joint 'Herbie/J1_shoulder1'
So I need to set joint limits in the robot arm xacro somehow. For a typical xacro file you can use <limit> to set things like this. For example, the 1st joint of the well-known Panda robot urdf is defined like so:

Code: Select all

<joint name="panda_joint1" type="revolute">
     <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
     <origin rpy="0 0 0" xyz="0 0 0.333" />
     <parent link="panda_link0" />
     <child link="panda_link1" />
     <axis xyz="0 0 1" />
     <limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.3925" />
</joint>
Hebi aims to simplify xacro generation, but I'm not sure how to set certain parameters such as <limit>. If there is documentation for this, let me know as I haven't encountered it yet. There seems to be integration in the hrdf -> xacro conversion, but I can't work out how to define things exactly. For example, in actuator.xacro, there is a section that can read user-defined limits, making a joint revolute rather than continuous:

Code: Select all

<!-- Continuous joints (no position limits) -->
<xacro:if value="${limits == []}">
  <limit effort="${effort_limit}" velocity="${velocity_limit}"/>
</xacro:if>
<!-- Revolute joints (position limits) -->
<xacro:unless value="${limits == []}">
  <limit effort="${effort_limit}" velocity="${velocity_limit}" lower="${limits[0]}" upper="${limits[1]}"/>
</xacro:unless>
This question ended up being more text than I originally intended, so let me get to the point: How do you define joint limits for actuators in a Hebi arm? If you can do that in the hrdf, that would be great. If I need to manually edit the xacro, that would also work. Thanks!
Post Reply