followjointTrajectory actions

[ilaro-org]

hi

is it possible to create a followjointTrajectory action ?

Header header
actionlib_msgs/GoalID goal_id
FollowJointTrajectoryGoal goal
================================================================================
MSG: std_msgs/Header

# sequence ID: consecutively increasing ID 
uint32 seq
time stamp
string frame_id
================================================================================
MSG: actionlib_msgs/GoalID

string id
================================================================================
MSG: control_msgs/FollowJointTrajectoryGoal

trajectory_msgs/JointTrajectory trajectory
JointTolerance[] path_tolerance
JointTolerance[] goal_tolerance
duration goal_time_tolerance
================================================================================
MSG: trajectory_msgs/JointTrajectory

Header header
string[] joint_names
JointTrajectoryPoint[] points
================================================================================
MSG: trajectory_msgs/JointTrajectoryPoint

float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start

this is what i tried

goal = roslibpy.actionlib.Goal(action_client,
        roslibpy.Message({
            dict(
                header=roslibpy.Header(seq=1, stamp=roslibpy.Time(now_sec, 0), frame_id=''),
                goal_id=1,
                goal=dict(
                    trajectory=
                    dict(
                        header=roslibpy.Header(seq=1, stamp=roslibpy.Time(now_sec+1, 0), frame_id=''), #roslibpy.Time.now()
                        joint_names=['joint_1','joint_2','joint_3','joint_4', 'joint_5', 'joint_6'],
                        points=[
                            dict(
                                    positions=[0,0,0,0,0,0],
                                    #velocities=[1,1,1,1,1,1],
                                    #accelerations=[1,1,1,1,1,1],
                                    # effort=[],
                                    time_from_start=roslibpy.Time(now_sec+2, 0),
                                )
                            ],
                        ),
                    # control_msgs/JointTolerance[] path_tolerance
                    # control_msgs/JointTolerance[] goal_tolerance
                    goal_time_tolerance = roslibpy.Time(now_sec+100, 0),
                    ),
                )
            })
        )

i think my python skills are too bad

would it be possible to have a more advance example like this? i couldn't find an example in the tests folder

thanks in advance

[beverlylytle]

Hi! Thanks for making an issue on roslibpy! The python looks good, and seems to define a valid followJointTrajectory message. Is it the case that you send this message, but nothing appears to happen? Because the only potential issue I see in the message is the confusion between Time and Duration. If now_sec is something like roslibpy.Time.now().to_sec(), then the defined joint trajectory has a duration time_from_start of approximately 51 years (plus 2 seconds), when it seems like maybe you mean it to be only 2 seconds. Similarly, goal_time_tolerance is a duration rather than a time stamp. Does this help?

[ilaro-org]

hi sorry, i think i wasn't clear enough

python throws this error TypeError: unhashable type: 'dict'

so, i'm a bit lost here, i don't know if nesting dicts like i'm doing is the proper way i also checked #47 but i wouldn't know how to proceed but i guess i'm in the right path, and it's about fixing the error (which i don't fully udnerstand)

maybe a solution to the problem is implementing classes like for example roslibpy.Header (added recently)?

# FollowJointTrajectoryGoal.msg
#
# trajectory_msgs/JointTrajectory trajectory
# control_msgs/JointTolerance[] path_tolerance
# control_msgs/JointTolerance[] goal_tolerance
# duration goal_time_tolerance

using Header as a template...

class Goal(UserDict):
    """Represents a message header of the ROS type std_msgs/Header."""
    def __init__(self, trajectory=None, path_tolerance=[], goal_tolerance=[], goal_time_tolerance=None):
        self.data = {}
        self.data['trajectory'] = trajectory
        self.data['path_tolerance'] = path_tolerance
        self.data['goal_tolerance'] = goal_tolerance
        self.data['goal_time_tolerance'] = goal_time_tolerance

is this the right way to solve this problem?

thanks a lot!

[ilaro-org]

and definitely the time/duration issue is something i've been trying to sort out, thanks for the explanation, definitely super valuable

[beverlylytle]

Aha! I think the TypeError is coming from an extra set of curly braces wrapping the the outermost dictionary. That's super easy to overlook. Try this instead

goal = roslibpy.actionlib.Goal(action_client,
        roslibpy.Message(
            dict(
                header=roslibpy.Header(seq=1, stamp=roslibpy.Time(now_sec, 0), frame_id=''),
                goal_id=1,
                goal=dict(
                    trajectory=
                    dict(
                        header=roslibpy.Header(seq=1, stamp=roslibpy.Time(now_sec+1, 0), frame_id=''), #roslibpy.Time.now()
                        joint_names=['joint_1','joint_2','joint_3','joint_4', 'joint_5', 'joint_6'],
                        points=[
                            dict(
                                    positions=[0,0,0,0,0,0],
                                    #velocities=[1,1,1,1,1,1],
                                    #accelerations=[1,1,1,1,1,1],
                                    # effort=[],
                                    time_from_start=roslibpy.Time(2, 0),
                                )
                            ],
                        ),
                    # control_msgs/JointTolerance[] path_tolerance
                    # control_msgs/JointTolerance[] goal_tolerance
                    goal_time_tolerance = roslibpy.Time(100, 0),
                    ),
                )
            )
        )

[ilaro-org]

great! it was that indeed

actually i had to remove the header and goal_id and goal keys, so it ended like this!

    goal = roslibpy.actionlib.Goal(action_client,
        roslibpy.Message(
            dict(
                # header=roslibpy.Header(seq=1, stamp=roslibpy.Time(now_sec, 0), frame_id=''),
                # goal_id= dict(stamp='', id=''), #stamp=roslibpy.Time(now_sec, 0), frame_id='holi'),
#                goal=dict(
                    trajectory=
                    dict(
                        header=roslibpy.Header(seq=1, stamp=roslibpy.Time(now_sec+1, 0), frame_id=''), #roslibpy.Time.now()
                        joint_names=['joint_1','joint_2','joint_3','joint_4', 'joint_5', 'joint_6'],
                        points=point_array,
                        ),
                    # control_msgs/JointTolerance[] path_tolerance
                    # control_msgs/JointTolerance[] goal_tolerance
                    goal_time_tolerance = roslibpy.Time(100, 0),
#                    ),
                )
            )
        )

thanks so so so much!

[beverlylytle]

Glad that worked out!