Implimenting joint limits from the joint_limits.yaml

[Nick Pestell]

Hello all,

I am trying to implement joint limits on a ur5 robot arm. Specifically joint position limits. I've updated the joint_limits.yaml to the desired values and I've confirmed that parameters have been loaded into the rosparam server correctly. However upon execution the robot happily violates the joint position parameters that I've set.

Am I missing something. Do I need some extra configuration such as using the joint_limits_interface c++ API to load the liimits and implement on the hardware?

Is there a better way of doing this such as specifying the joint limits in the urdf? I have tried to do this but again the robot seems to violate the positions that I've specified.

Thanks in advance.

Nick

[v4hn]

On Wed, May 04, 2016 at 03:35:25AM -0700, Nick Pestell wrote:
> I am trying to implement joint limits on a ur5 robot arm. Specifically
> joint position limits. I've updated the joint_limits.yaml to the desired
> values and I've confirmed that parameters have been loaded into the
> rosparam server correctly.

What exactly are you trying to do/achive?

> However upon execution the robot happily
> violates the joint position parameters that I've set.

What does "the robot" mean? *moveit* is supposed to respect the limits,
not your controller.


v4hn

[Nick Pestell]

Thanks for your reply.
 

What exactly are you trying to do/achive?

At the moment I just want move the robot in order to check that my joint position limits are being implemented. I publish a joint position to follow_joint_trajectory/goal which is outside of the limits I set in joint_limits.yaml. I'm expecting that the robot can't move to this goal state due to the limit however it moves there no problem.

What does "the robot" mean? *moveit* is supposed to respect the limits,
not your controller.

 

By "the robot" I mean just that, the actual hardware moves to the goal position despite violating limits in joint_limits.yaml.

I think I see what you're getting at; in the set up I've described above I'm actually not using moveit to do any planning. It's moveit which uses  "robot_description_planning/joint_limits" parameters?

Nick

[Nick Pestell]

I've just tested using the GetPositionIK moveit service. Moveit is choosing joint positions which violate joint_limits.yaml and again the robot is able to achieve these joint positions.

Nick

On Wednesday, May 4, 2016 at 12:27:56 PM UTC+1, v4hn wrote:

[v4hn]

On Wed, May 04, 2016 at 05:00:48AM -0700, Nick Pestell wrote:
> I think I see what you're getting at; in the set up I've described above
> I'm actually not using moveit to do any planning. It's moveit which uses
> "robot_description_planning/joint_limits" parameters?

Yes. ur_driver's follow_joint_trajectory action is ignorant of these parameters.

> I've just tested using the GetPositionIK moveit service. Moveit is choosing
> joint positions which violate joint_limits.yaml and again the robot is able
> to achieve these joint positions.

Hm, I thought this should work, but it probably depends on your IK solver.
Which one are you using? (btw: Did you get universal_robot's UR5KinematicsPlugin to work?)

Skipping through the code,

- move_group instantiates a PlanningScene+PlanningSceneMonitor
- the monitor initializes a RobotLoader
- the joint limits are read by the RobotLoader and used to instantiate
the resp. JointModels in RobotModel
- the RobotModel initializes JointModelGroup with these JointModels + sets a solver
- the PlanningScene initializes RobotStates with this RobotModel
- GetPositionIK uses this RobotState
- the RobotState uses the jointmodelgroup's solver
- The solver ignores the joint's limits...

Does *planning to move* to that out-of-limits-state (not just inverse kinematics)
work as well? I'm not sure where exactly the limits kick in in this
totally intuitive and easy to survey architecture...

Best,


v4hn

[Daniel Solomon]

Limits (in general) aren't checked outside of the planning components of MoveIt. Most planners will utilize isStateValid() or similar methods, and those will check the limits (set in the urdf, overrided by parameters).

I don't know what interface you are using to MoveIt, but you should see if there is a state validity check you can use. Otherwise, the method you are describing doesn't have any way of utilizing the limits.

[Nick Pestell]

Thank you for your summary! 

Does *planning to move* to that out-of-limits-state (not just inverse kinematics) 
work as well? 

 

I'm sorry but I'm not quite sure what you mean here. The inverse kinematics works out the joint positions, and then the planning process works out how to move the robot to these joints positions? Presumably since the robot is actually moving to the configuration then the planning works? How would I differentiate between the blocks?  

Hm, I thought this should work, but it probably depends on your IK solver. 
Which one are you using?

AFAIK I'm just using the GetPositionIK.srv, does that help? Perhaps it's useful to look at my code quickly;

import rospy
import roslib
import actionlib
import inverse_kinematics #LIBRARY CONTAINING THE 'ik_client' CLASS
from control_msgs.msg import *
from trajectory_msgs.msg import*
from sensor_msgs.msg import JointState
from math import cos, sin, pi

#PERFORMS INVERSE KINEMATICS ON SPECIFIED CARTESIAN COORDINATES AND EULER ANGLES
def find_joint_values(i,j,k,ai,aj,ak):

 #INSTANTIATE AN OBJECT, 'joint_values' WITH ATRIBUTES FROM THE 'IK_client' CLASS
 joint_values = inverse_kinematics.IK_client()

 #CALL AND RETURN 'get_joint_positions' FUNCTION FROM 'IK_client' CLASS
 return joint_values.get_joint_positions(i,j,k,ai,aj,ak)

#CONNECTS TO DRIVER VIA THE 'follow_joint_trajectory' ACTION AND PUBLISHES JOINT VALUES TO
#'follow_joint_trajectory/goal' ACTION TOPIC.
def move_robot(goal):
 try:

 #INSTANTIATE A CLIENT OBJECT - SimpleActionClient IS A PYTHON CLASS IN THE ACRTIONLIB
 #API. __innit__(self, topic name, message type).
 client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction) #initialize the action client

 #CREATE VARIABLE 'pose' OF MESSAGE TYPE 'FollowJointTrajectoryGoal' TO STORE GOAL
 #STATE WITH FIELDS FOR JOINT NAMES AND JOINT POSITIONS IN RADIANS.
 pose = FollowJointTrajectoryGoal()
 pose.trajectory = JointTrajectory()

 #GET CURRENT STATE
 startstate = rospy.wait_for_message('joint_states', JointState)
 start_pose = startstate.position

 #POPULATE 'pose' WITH JOINT NAMES AND GOAL POSITIONS. (INITIAL GOAL IS SET TO CURRENT
 #STATE TO INSURE OPPERATION), TARGET GOAL IS COMPUTED BY 'find_joint_values' FROM THE
 #CARTESIAN COORDINATES IN main().
 pose.trajectory.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
 #pose.trajectory.joint_names = goal.solution.joint_state.name
 pose.trajectory.points = [JointTrajectoryPoint(positions= start_pose, velocities=[0]*6, time_from_start=rospy.Duration(0.0)),
 JointTrajectoryPoint(positions= goal.solution.joint_state.position, velocities=[0]*6, time_from_start=rospy.Duration(20.0))]

 #WAIT FOR SERVER ON DRIVER AND SEND GOAL- 'pose' WHEN AVAILABLE
 client.wait_for_server()
 client.send_goal(pose)
 client.wait_for_result()

 return client.get_result()

 except KeyboardInterrupt:
        client.cancel_goal()
    raise
    except:
    raise

def main():
 #ENTER THE DESIRED x,y,z - CARTESIAN COORDIANTES
 x = 0.6
 y = 0.6
 z = 0.3

 #ENTER THE DESIRED roll,pitch,yaw EULER ANGLES
 roll = 0
 pitch = pi/2
 yaw = 0

 try:
 #INITIALIZE THE 'ur5_client' NODE
 rospy.init_node('ur5_client')

 #FIND THE JOINT VALUES FROM THE SPECIFIED CARTESIAN COORDINATES AND EULER ANGLES
 joint_values = find_joint_values(x,y,z,roll,pitch,yaw)

 #PUBLISH THE GOAL STATE ON THE FollowJointTrajectory ACTION
 move_robot(joint_values)

 except KeyboardInterrupt:
 rospy.signal_shutdown("KeyboardInterrupt")
 raise
        print "program stopped"

if __name__ == '__main__':
 main()

My IK service client;

import rospy
from moveit_msgs.srv import GetPositionIK #MOVEIT SERVICE FOR PERFORMING INVERSE KINEMATICS
from moveit_msgs.msg import *
from math import cos, sin, pi
from tf import transformations
from geometry_msgs.msg import *

class IK_client(object):

  #TAKES CARTESIAN COORDINATES AND EULER ANGLES, PERFORMS IK, AND RETURNS A SET OF JOINT
  #ANGLES
  def get_joint_positions(self,x,y,z,roll,pitch,yaw):

 #CREATE VARIABLE 'target' OF MESSAGE TYPE 'PoseStamped()' FOR STORING THE TARGET
 #CARTESIAN AND QUATERNION ANGLES
 target = PoseStamped()

 #POPULATE 'target' WITH THE CARTESIAN COORDINATES
 target.pose.position.x = x
 target.pose.position.y = y
 target.pose.position.z = z

 #TRANSFORM THE EURLER ANGLES TO THE REQUIRED QUATERNIONS
 q = transformations.quaternion_from_euler(roll, pitch, yaw, "sxyz")

 #POPULTAE 'target' WITH THE QUATERNION ANGLES
 target.pose.orientation.x = q[0]
 target.pose.orientation.y = q[1]
 target.pose.orientation.z = q[2]
 target.pose.orientation.w = q[3]

 #INSTANTIATE AND OBJECT 'compute_ik' WITH ATTRIBUTES FROM THE 'ServiceProxy'
 #CLASS. __innit__(self, name of service, service class). THIS IT THE
 #'GetPositionIK' SERVICE
 compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)

 #CREATE A VARIABLE 'posreq' WITH THE MESSAGE TYPE 'PositionIKRequest' AND
 #POPULATE WITH THE NECESSARY VALUES
 posreq = PositionIKRequest()
 posreq.group_name = 'manipulator'
 posreq.pose_stamped = target #THIS IS THE ATTRIBUTE FOR POSITION AND ORIENTATION
 posreq.ik_link_name = 'ee_link'
 posreq.timeout.secs = 0.005
 posreq.avoid_collisions = True
 #posreq.attempts = 0

 #THIS CALLS THE 'GetPositionIK' SERVICE WITH 'posreq' AS ARGUMENT AND RETURNS THE
 #RESPONSE
 resp = compute_ik(ik_request = posreq)
 return resp

(btw: Did you get universal_robot's UR5KinematicsPlugin to work?) 

I have not tried.

Thanks,

Nick

[Nick Pestell]

Hi Daniel,

I think I accidentally sent the wrong reply to you previously... 

Thank you for your response. Please see my previous reply for my code. I'm basically just using the GetPositionIK service to work out the inverse kinematics from a set of Cartesian coordinates and Euler angles and then publishing the resulting joint angles straight to /FollowJointTrajectroy/goal. 

Presumably isStateValid() wouldn't be used in the inverse kinematics? So I suppose I'm going to have to use a different interface to moveit in order to implement these limits? Such as moveit_commander? I'm not entirely sure of the extra features in the whole workflow when using this interface. Obviously I would be using functions from the moveit_commander API, but I would have assumed that the stuff going on behind the scenes is essentially the same i.e. IK and communication with the ur_driver.

Many thanks 

Nick   

[Nick Pestell]

So I have just tested using the moveit_commander API, specifically MoveGroupCommander.plan() and MoveGroupCommander.execute(), and this seems to honour the limits from joint_limits.yaml. I'm wondering if either of you could possibly help me to understand where the differences between the two methods (basic FollowJointTrajectoryAction client (A) vs. using the moveit_commander interface (B)) come in that is causing (B) to honour limits and not (A)

Most planners will utilize isStateValid() or similar methods, and those will check the limits (set in the urdf, overrided by parameters).

Clearly, as you say, with (B) there is some phase (planning?) which includes the joint validity checks, but what I'm slightly confused about is the fact that when I'm using (A) there must also be planning too, no? Is it just the case that the planning in this technique doesn't include isStateValid()? 

If there is planning when using (A) at what stage does it occur? If I simply give the driver joint values over follow_joint_trajectory/goal then I can't see at what stage planning would occur since the driver itself has no planning capabilities, right?

If they help at all, below are the rqt_graphs of set ups (A) and (B) respectively, where ur5_client and set_pose are my own programs.

  

I'm inclined to think that perhaps using method (A) there is no planning as such and I simply send a joint configuration to the driver which implements it on the robot without any thought of how to get to this joint position.

I have noticed that when using (B) different goal are sequentially published on follow_joint_trajectory/goal at a frequency of about 4hz throughout an execution, whereas with (B) obviously only one single goal state is published. Is this indicative of the fact that there is planning in (B) and not in (A)?

I hope that all makes sense, any help you can offer is really appreciated!!

Thanks,

Nick  

On Wednesday, May 4, 2016 at 7:47:34 PM UTC+1, Daniel Solomon wrote:

[Daniel Solomon]

1. As you have seen previously, the getPositionIK service access a lower-level function in MoveIt that does not consider joint limits.

2. The ur driver does not consider joint limits when sending commands to the robot controller. It is assumed at that point that you have created your trajectory in a valid manner.

So in effect you have assigned arbitrary joint limits, but are using components that do not consider joint limits in their function.

When you used MoveGroupCommander.plan, that invoked a higher-level planner that, as part of its function, checks validity at each state.  Note that the planner's definition of validity encompasses joint limits, collision, and optional user-defined limits. Using the getPositionIK service, and your own simple "planner" (move from current state to state calculated by IK) does not take these limits into account.

[Nick Pestell]

Great thanks for your reply. 

Cheers,

Nick