What is the best way to control Sawyer robot if I already have the trajectory?
Hello everyone!
Currently, I am working with Sawyer robotic arm and coded up an artificial intelligence code in Matlab to find a trajectory that avoids obstacles and minimizes control effort (joint movement). Now, I have the joint space trajectory (Sawyer has 7 joints, so this is a 7x100 matrix, 100 points), a time vector (1x100) and I can also obtain joint speeds and joint accelerations by simply differentiating. My question is: what is the best way to use this information (7x100 trajectory) to control the real robot? I have the actual robotic arm, but I started working in Gazebo and my first try was using MoveIt!. I thought that it would be helpful to ask this question here before trying anything ineffective. Thank you!
Edit: Thank you for your replies! I tried setting up a simple code and eventually succeeded in controlling the joint positions using limb.move_to_joint_positions
. Also, I found that there is another command that can help me
limb.set_joint_trajectory
. However, it does not work - after I run the Python script the Robot does not move. Has anyone used limb.set_joint_trajectory
before? I attached my piece of code below where I am trying to use the existing trajectory on joint 2. Possibly I am not using the self._limb.set_joint_trajectory('right_j2',pos,vel,acc)
correctly. I could not find anything close to this problem. The self._limb.move_to_joint_positions({'right_j0':0,'right_j1':0, 'right_j2':0,'right_j3':0,'right_j4':0,'right_j5':0,'right_j6':0})
are working just fine, that is why I commented them out of the attached code.
#!/usr/bin/env python
# Import math modules
import math
import numpy as np
# Import robot interface modules
import rospy
import intera_interface
from intera_interface import CHECK_VERSION
# Initialize robot
rospy.init_node("Test")
intera_interface.RobotEnable(
CHECK_VERSION).enable()
# Main class
class Main:##
# Initialize application
def __init__(self):
# Create robot
self.robot = Robot()
self.robot.run()
# Robot
class Robot(object):
# Initialize window
def __init__(self):
self._limb = intera_interface.Limb()
self._target_angle = 0
def run(self):
# Move to neutral position
print("Moving to neutral...")
#self._limb.move_to_neutral()
#self._limb.set_joint_position_speed(1)
#self._limb.move_to_joint_positions({'right_j0':0,'right_j1':0, 'right_j2':0,'right_j3':0,'right_j4':0,'right_j5':0,'right_j6':0})
#self._limb.move_to_joint_positions({'right_j0':1,'right_j1':0, 'right_j2':0,'right_j3':0,'right_j4':0,'right_j5':0,'right_j6':0})
#self._limb.move_to_joint_positions({'right_j0':0,'right_j1':0, 'right_j2':0,'right_j3':0,'right_j4':0,'right_j5':0,'right_j6':0})
#self._limb.move_to_joint_positions({'right_j0':1,'right_j1':0, 'right_j2':0,'right_j3':0,'right_j4':0,'right_j5':0,'right_j6':0})
#self._limb.move_to_joint_positions({'right_j0':0,'right_j1':0, 'right_j2':0,'right_j3':0,'right_j4':0,'right_j5':0,'right_j6':0})
pos = [0,0.278521147421560,0.334311794594397,0.418267057180151,0.419262882688881,0.422556967444284,0.425164455138498,0.431006517211002,0.444412714223080,0.467013462886176,0.487570813194810,0.521244082754868,0.572498161392207,0.615699683910449,0.652258704653838,0.688486332506119,0.712269045528898,0.714783340982971,0.753313779601416,0.790976143848783,0.828967448333042,0.871415882700251,0.920906176488521,0.936397404556755,0.979800613969219,1.02083077359423]
vel=[2.18381763515172,0.730617806133591,0.258549873450524,0.120452729287060,0.0701927980567305,0.0397948807240920,0.0246668664822685,0.0316916864038248,0.0612174375119867 ...
Just to clarify: you can actually send trajectories to Sawyer without MoveIt.
At a high-level, you'd:
FollowJointTrajectory
action clientGoal
JointTrajectory
@fvd mentions inside thatGoal
messageFollowJointTrajectory
serverMatlab supports action client in their ROS Toolbox. If you have it, it may be the leanest way to control Sawyer from Matlab.