def _pub_joint_velocities(self, velocities):
command_msg = JointCommand()
command_msg.names = self.arm_joint_names
command_msg.velocity = velocities
command_msg.mode = JointCommand.VELOCITY_MODE
command_msg.header.stamp = rospy.Time.now()
self.joint_pub.publish(command_msg)
def _pub_joint_torques(self, torques):
command_msg = JointCommand()
command_msg.names = self.arm_joint_names
command_msg.effort = torques
command_msg.mode = JointCommand.TORQUE_MODE
command_msg.header.stamp = rospy.Time.now()
self.joint_pub.publish(command_msg)
def _pub_joint_positions(self, positions):
command_msg = JointCommand()
command_msg.names = self.arm_joint_names
command_msg.position = positions
command_msg.mode = JointCommand.POSITION_MODE
command_msg.header.stamp = rospy.Time.now()
self.joint_pub.publish(command_msg)
def _set_joint_velocities(self, actions):
if self.vel_mode == "raw":
velocities = dict()
enum_iter = enumerate(self.cmd_names, start=0)
for i, jnt in enum_iter:
velocities[jnt] = actions[i]
command_msg = JointCommand()
command_msg.names = velocities.keys()
command_msg.velocity = velocities.values()
command_msg.mode = JointCommand.VELOCITY_MODE
command_msg.header.stamp = rospy.Time.now()
self.jnt_cm_pub.publish(command_msg)
else:
# Command Sawyer's joints to angles as calculated by velocity*dt
positions = dict()
q_jnt_angles = copy.copy(self.init_pos)
obs_prev = self._get_cur_obs()
enum_iter = enumerate(self.jnt_indices, start=0)
for i, index in enum_iter:
timestep = self.dt + np.random.normal(0, 1)
val = obs_prev[i] + actions[i] * timestep
val = np.clip(val, self.jnt_pos_limits[i][0],
self.jnt_pos_limits[i][1])
q_jnt_angles[index] = val
enum_iter = enumerate(self.all_jnts, start=0)
for i, jnt_name in enum_iter:
positions[jnt_name] = q_jnt_angles[i]
self.limb.move_to_joint_positions(positions)
def basicPositionMoveForPositioning(self, pos, speed):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
rate = rospy.Rate(180)
# define the speed publisher
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [
pos['right_j0'], pos['right_j1'], pos['right_j2'], pos['right_j3'],
pos['right_j4'], pos['right_j5'], pos['right_j6']
]
command.mode = 1
# customize the value to change speed
pub_speed_ratio.publish(speed)
# terminate the control once the arm moved to the desired joint space within the threshold
pub.publish(command)
rate.sleep()
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def movetozero(self):
rospy.init_node('arm_low_level_control', anonymous=True)
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
command.mode = 1
pub_speed_ratio.publish(0.2)
start_time = rospy.get_time()
end_time = rospy.get_time()
rospy.loginfo(
">>>>>>>>>> moving the arm to zero joint position >>>>>>>>>>>")
while end_time - start_time < 8:
pub.publish(command)
end_time = rospy.get_time()
def main():
try:
rospy.init_node('avalos_limb_py')
rate = rospy.Rate(100)
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
#limb = Limb()
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 1
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"
]
my_msg.position = [0, 0, 0, 0, 0, 0, 0]
for x in xrange(100):
pub.publish(my_msg)
rate.sleep()
print "Move ok"
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def _send_pos_command(self, pos):
self._try_enable()
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = self._limb.joint_names()
command.position = pos
self._cmd_publisher.publish(command)
def bring2stretched(self): _robot = JointCommand() _robot.mode = 1 _robot.names = ['right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6'] _robot.position = [0,0,-1.61,0,0,0,0] for i in range(1000): self.pub_joints.publish(_robot) time.sleep(0.01)
def talker2():
rospy.init_node('joint_state_publisher_0')
pub = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10)
rate = rospy.Rate(10) # 10hz
hello_str = JointCommand()
hello_str.header = Header()
hello_str.header.stamp = rospy.Time.now()
hello_str.mode = 1
hello_str.names = ['right_j0', 'right_j1', 'right_j2', 'right_j3',
'right_j4', 'right_j5', 'right_j6']
# hello_str.position = [-0.20288, 0.61753, -1.74399, -1.32579, 1.09791, -1.74968, 0.51062]
# hello_str.position = [-1.20675, 0.467, -3.04262, -0.39422, 2.96331, -2.21461, 2.87877]
positions = [[-0.20288, 0.61753, -1.74399, -1.32579, 1.09791, -1.74968, 0.51062], [-0.36308, 0.48712, -1.88532, -1.08158, 1.30146, -1.8188, 0.22098], [-0.39752, 0.44012, -1.93739, -0.98365, 1.25842, -1.75613, 0.24877], [-0.43197, 0.39311, -1.98946, -0.88573, 1.21539, -1.69347, 0.27656], [-0.46642, 0.3461, -2.04154, -0.7878, 1.17236, -1.6308, 0.30435], [-0.50087, 0.29909, -2.09361, -0.68987, 1.12933, -1.56814, 0.33214], [-0.53532, 0.25209, -2.14568, -0.59195, 1.08629, -1.50547, 0.35993], [-0.56976, 0.20508, -2.19776, -0.49402, 1.04326, -1.4428, 0.38772], [-0.60421, 0.15807, -2.24983, -0.3961, 1.00023, -1.38014, 0.41551], [-0.63936, 0.16653, -2.29415, -0.3935, 1.11641, -1.42953, 0.56089], [-0.67451, 0.17499, -2.33848, -0.39091, 1.2326, -1.47893, 0.70627], [-0.70966, 0.18346, -2.3828, -0.38832, 1.34879, -1.52832, 0.85165], [-0.74481, 0.19192, -2.42712, -0.38573, 1.46497, -1.57772, 0.99702], [-0.77996, 0.20038, -2.47145, -0.38314, 1.58116, -1.62711, 1.1424], [-0.81511, 0.20884, -2.51577, -0.38055, 1.69735, -1.67651, 1.28778], [-0.85026, 0.2173, -2.56009, -0.37796, 1.81354, -1.7259, 1.43316], [-0.88541, 0.22576, -2.60442, -0.37536, 1.92972, -1.7753, 1.57853], [-0.92056, 0.23422, -2.64874, -0.37277, 2.04591, -1.82469, 1.72391], [-0.95571, 0.24268, -2.69306, -0.37018, 2.1621, -1.87409, 1.86929], [-0.99086, 0.25114, -2.73739, -0.36759, 2.27828, -1.92348, 2.01467], [-1.02601, 0.2596, -2.78171, -0.365, 2.39447, -1.97288, 2.16004], [-1.06116, 0.26806, -2.82603, -0.36241, 2.51066, -2.02227, 2.30542], [-1.0963, 0.27652, -2.87036, -0.35981, 2.62684, -2.07167, 2.4508], [-1.13145, 0.28498, -2.91468, -0.35722, 2.74303, -2.12106, 2.59618], [-1.1666, 0.29344, -2.959, -0.35463, 2.85922, -2.17045, 2.74155], [-1.20175, 0.30191, -3.00333, -0.35204, 2.9754, -2.21985, 2.88693]]
hello_str.velocity = []
hello_str.effort = []
# pub.publish(hello_str)
# time.sleep(10)
step = 0
while not rospy.is_shutdown():
hello_str.header.stamp = rospy.Time.now()
hello_str.position = positions[step]
pub.publish(hello_str)
step +=1
if(step >= len(positions)):
break
time.sleep(2)
rate.sleep()
def lowerArmBasicMove(self, cur_pos, speed):
# define some parameters:
frequency = 100 # customize this to change the vibration speed
repetation = 20
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
rate = rospy.Rate(frequency)
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
# set the joint angles for the first press position
command1 = JointCommand()
command1.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command1.position = [
cur_pos['right_j0'], cur_pos['right_j1'], cur_pos['right_j2'],
cur_pos['right_j3'] - 0.05, cur_pos['right_j4'] - 0.05,
cur_pos['right_j5'] + 0.1, cur_pos['right_j6']
]
command1.mode = 1
# set the joint angles for the second press position
command2 = JointCommand()
command2.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command2.position = [
cur_pos['right_j0'], cur_pos['right_j1'], cur_pos['right_j2'],
cur_pos['right_j3'], cur_pos['right_j4'], cur_pos['right_j5'],
cur_pos['right_j6']
]
command2.mode = 1
# looping to create vibrating motion
pub_speed_ratio.publish(speed)
for i in range(0, repetation):
self.forcePress(self.vibration_force, 0.1)
#for j in range(0, 10):
# pub.publish(command1)
# rate.sleep()
#rate.sleep()
for k in range(0, 10):
pub.publish(command2)
rate.sleep()
rate.sleep()
def _set_joint_torques(self, actions):
torques = dict()
enum_iter = enumerate(self.all_jnts, start=0)
for i, jnt_name in enum_iter:
torques[jnt_name] = 0.0
enum_iter = enumerate(self.cmd_names, start=0)
for i, jnt_name in enum_iter:
torques[jnt_name] = actions[i]
command_msg = JointCommand()
command_msg.names = torques.keys()
command_msg.effort = torques.values()
command_msg.mode = JointCommand.TORQUE_MODE
command_msg.header.stamp = rospy.Time.now()
self.jnt_cm_pub.publish(command_msg)
def basicTrajMove(self, positions, speed, traj_length, speed_rate):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
rate = rospy.Rate(speed_rate)
# define the speed publisher
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.mode = 1
# customize the value to change speed
pub_speed_ratio.publish(speed)
control_diff_record = 10.0
control_diff_temp = 0.0
threshold = 0.015
# terminate the control once the arm moved to the desired joint space within the threshold
counter = 0
while control_diff_record > threshold and counter < traj_length - 1:
if positions[counter] == False:
continue
command.position = [
positions[counter]['right_j0'], positions[counter]['right_j1'],
positions[counter]['right_j2'], positions[counter]['right_j3'],
positions[counter]['right_j4'], positions[counter]['right_j5'],
positions[counter]['right_j6']
]
pub.publish(command)
for x, y in zip(self.jointAngles, command.position):
control_diff_temp = abs(x - y) + control_diff_temp
control_diff_record = control_diff_temp
control_diff_temp = 0.0
rate.sleep()
counter = counter + 1
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicPositionMove(self, pos, speed):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
rate = rospy.Rate(180)
# define the speed publisher
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [
pos['right_j0'], pos['right_j1'], pos['right_j2'], pos['right_j3'],
pos['right_j4'], pos['right_j5'], pos['right_j6']
]
command.mode = 1
# customize the value to change speed
pub_speed_ratio.publish(speed)
control_diff_record = 10.0
control_diff_temp = 0.0
threshold = 0.02
# terminate the control once the arm moved to the desired joint space within the threshold
start_time = rospy.get_time()
end_time = rospy.get_time()
while control_diff_record > threshold:
end_time = rospy.get_time()
if end_time - start_time > 5:
break
pub.publish(command)
for x, y in zip(self.jointAngles, command.position):
control_diff_temp = abs(x - y) + control_diff_temp
control_diff_record = control_diff_temp
control_diff_temp = 0.0
rate.sleep()
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicPositionMove(self, pos, speed, initial):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
# define the speed publisher
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [
pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
]
command.mode = 1
# customize the value to change speed
pub_speed_ratio.publish(speed)
# terminate the control once the arm moved to the desired joint space within the threshold
rate = rospy.Rate(1000)
control_diff_record = 10.0
control_diff_temp = 0.0
threshold = 0.02
if not initial:
# terminate the control once the arm moved to the desired joint space within the threshold
for i in range(0, 200):
pub.publish(command)
rate.sleep()
else:
for i in range(0, 10000):
rospy.logerr("returning to the first point")
pub.publish(command)
rate.sleep()
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def basicAccelerationMove(self, pos, speed):
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
pub_speed_ratio = rospy.Publisher('/robot/limb/right/set_speed_ratio',
Float64,
latch=True,
queue_size=10)
sub = rospy.Subscriber('/robot/joint_states', JointState,
self.joints_callback)
command = JointCommand()
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
command.position = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
command.acceleration = [0.3, 0.05, 0.05, 0.05, 0.05, 0.05, 0.1]
command.mode = 1
#pub_speed_ratio.publish(0.2)
rate = rospy.Rate(10)
control_diff_record = 10.0
control_diff_temp = 0.0
threshold = 0.02
# terminate the control once the arm moved to the desired joint space within the threshold
while control_diff_record > threshold:
pub.publish(command)
for x, y in zip(self.jointAngles, command.acceleration):
control_diff_temp = abs(x - y) + control_diff_temp
control_diff_record = control_diff_temp
control_diff_temp = 0.0
rate.sleep()
rospy.loginfo(
">>>>>>>>>> The robot is at the target position <<<<<<<<<<:")
def __init__(self):
rospy.loginfo("Creating IK Controller class")
# setup flags and useful vars:
self.running_flag = False
self.freq = rospy.get_param("~freq", FREQ)
self.arm = rospy.get_param("~arm", ARM)
self.base = rospy.get_param("~base", BASE)
self.target = rospy.get_param("~target", TARGET)
self.step_scale = rospy.get_param("~scale", STEP_SCALE)
self.tolerance = rospy.get_param("~tolerance", TOLERANCE)
self.damping = rospy.get_param("~damping", DAMPING)
self.center_x = rospy.get_param("~center_x", CENTER_X)
self.center_y = rospy.get_param("~center_y", CENTER_Y)
self.center_z = rospy.get_param("~center_z", CENTER_Z)
self.range_x = rospy.get_param("~range_x", RANGE_X)
self.range_y = rospy.get_param("~range_y", RANGE_Y)
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
with cl.suppress_stdout_stderr():
self.urdf = URDF.from_parameter_server()
self.kin = KDLKinematics(self.urdf, "base", "%s_hand"%self.arm)
self.q = np.zeros(7)
self.q_sim = np.zeros(7)
self.limb_interface = intera_interface.Limb()
self.center_pos()
self.goal = np.array(self.kin.forward(self.q))
self.js = JointState()
self.js.name = self.kin.get_joint_names()
self.pose = PoseStamped()
self.mutex = threading.Lock()
self.joint_cmd = JointCommand()
self.joint_cmd.names = self.kin.get_joint_names()
self.joint_cmd.mode = 2
# create all services:
self.toggle_server = rospy.Service("toggle_controller", SetBool, self.toggle_handler)
self.reset_server = rospy.Service("reset", Empty, self.reset_handler)
self.random_server = rospy.Service("random", Empty, self.random_handler)
# create all subscribers, timers, and publishers:
self.ref_sub = rospy.Subscriber("ref_pose", PoseStamped, self.refcb)
self.actual_js_sub = rospy.Subscriber("robot/joint_states", JointState, self.actual_js_cb)
self.js_pub = rospy.Publisher("joint_states", JointState, queue_size=3)
self.pose_pub = rospy.Publisher("pose", PoseStamped, queue_size=3)
self.joint_cmd_timeout_pub = rospy.Publisher("robot/limb/right/joint_command_timeout", Float64, queue_size=3)
self.center_pos()
self.joint_cmd_pub = rospy.Publisher("robot/limb/right/joint_command", JointCommand, queue_size=3)
self.int_timer = rospy.Timer(rospy.Duration(1/float(self.freq)), self.ik_step_timercb)
return
def __init__(self, config=None):
"""Initialize.
See the parent class.
"""
super(FrankaPandaReal, self).__init__(config=config)
if rospy.get_name() == '/unnamed':
rospy.init_node('franka_panda')
assert rospy.get_name() != '/unnamed', 'Must call rospy.init_node().'
tic = time.time()
rospy.loginfo('Initializing robot...')
self._head = intera_interface.Head()
self._display = intera_interface.HeadDisplay()
self._lights = intera_interface.Lights()
self._limb = intera_interface.Limb()
self._joint_names = self._limb.joint_names()
self.cmd = []
self._command_msg = JointCommand()
self._command_msg.names = self._joint_names
self._commanders = dict(velocity=None, torque=None)
try:
self._gripper = intera_interface.Gripper()
self._has_gripper = True
except Exception:
self._has_gripper = False
self._robot_enable = intera_interface.RobotEnable(True)
self._params = intera_interface.RobotParams()
self._motion_planning = self.config.MOTION_PLANNING
if self._motion_planning == 'moveit':
rospy.loginfo('Initializing moveit toolkit...')
moveit_commander.roscpp_initialize(sys.argv)
self._scene = moveit_commander.PlanningSceneInterface()
self._group = moveit_commander.MoveGroupCommander('right_arm')
toc = time.time()
rospy.loginfo('Initialization finished after %.3f seconds.'
% (toc - tic))
def __init__(self):
# Messages to set the goal position
self.seq = 1
self.dyn_point = Point()
self.dyn_quat = Quaternion()
self.goal_pose = PoseStamped()
self.joint_cmd = JointCommand()
self.joint_cmd.header.seq = self.seq
self.joint_cmd.mode = JointCommand.POSITION_MODE
self._joint_names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
self.joint_cmd.names = self._joint_names
# Create a publisher to send joint position commands
self.joint_publisher = rospy.Publisher(
'/robot/limb/right/joint_command', JointCommand, queue_size=5)
# Wait for subscribers to listen
while self.joint_publisher.get_num_connections() == 0:
pass
def __init__(self):
# Init node
rospy.init_node('sawyer_vel_ctrl')
# tflistener Init
self.tf_lis = tf.TransformListener()
# Trajectory Subscriber
rospy.Subscriber("/desired_trajectory", TransformStamped, self.ctrl_r)
# Joint States Subscriber
rospy.Subscriber("/robot/joint_states", JointState, self.js_store)
# Control Message Publisher
self.pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
## Control Gains
self.Kp = 2 * np.eye(6) # Proportional
self.Ki = 0 * np.eye(6) # Intergral
## Robot Description
self.B_list = s_des.Blist
self.M = s_des.M
# Set initial joint states to 0, may need to change in IRL use
# ...but you might not
self.cur_config = np.zeros(9)
# Wait for actual joint_states to be stored by js_store() callback
while np.sum(self.cur_config) is 0:
rospy.sleep(0.1)
# Joint Command Message Init
self.joint_ctrl_msg = JointCommand()
self.joint_ctrl_msg.names = ['right_j0', 'head_pan', 'right_j1', 'right_j2', \
'right_j3', 'right_j4', 'right_j5', 'right_j6']
self.joint_ctrl_msg.mode = VELOCITY_MODE
# Set 0 initial velocity at all joints
self.joint_ctrl_msg.velocity = \
np.ndarray.tolist(np.zeros(len(self.joint_ctrl_msg.names)))
self.it_count = 0 # Iteration count for debug
self.int_err = np.zeros(6) # Integrated error 0 -> t
def move_move_no(limb, group, target, speed_ratio=None, accel_ratio=None, timeout=None):
trajectory_publisher = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size = 1)
JointCommandMessage = JointCommand()
JointCommandMessage.mode = 4
if speed_ratio is None:
speed_ratio = 0.3
if girigiri_aiiiiii:
speed_ratio = 0.8
if accel_ratio is None:
accel_ratio = 0.1
if girigiri_aiiiiii:
accel_ratio = 0.2
plan = group.plan(target)
rospy.sleep(1)
JointCommandMessage.names = plan.joint_trajectory.joint_names
start_time = rospy.get_time()
position_array = []
velocity_array = []
acceleration_array = []
time_array = []
for point in plan.joint_trajectory.points:
position = point.positions
velocity = point.velocities
acceleration = point.accelerations
position_array.append(position)
velocity_array.append(velocity)
acceleration_array.append(acceleration)
desire_time = point.time_from_start.to_sec()
time_array.append(desire_time)
s_array = time_array
wp_array = position_array
path = ta.SplineInterpolator(s_array, wp_array)
s_sampled = np.linspace(0, time_array[len(time_array) - 1], 100)
q_sampled = path.eval(s_sampled)
# --------------------------- plotting the interpolator --------------------------
# plt.plot(s_sampled, q_sampled)
# plt.hold(True)
# plt.plot(time_array, position_array, 'kd')
# plt.legend(["joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7"])
# plt.hold(False)
# plt.show()
# exit()
# ---------------------------- end of plotting ----------------------------------
# Create velocity bounds, then velocity constraint object
dof = 7
vlim_ = np.ones(dof) * 5 #* speed_ratio
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(dof) * 2 * accel_ratio
alim = np.vstack((-alim_, alim_)).T
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(
alim, discretization_scheme=constraint.DiscretizationType.Interpolation)
# Setup a parametrization instance
instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel')
# Retime the trajectory, only this step is necessary.
t0 = time.time()
jnt_traj, aux_traj = instance.compute_trajectory(0, 0)
print("TOPPRA Parameterization time: {:} secs".format(time.time() - t0))
ts_sample = np.linspace(0, jnt_traj.get_duration(), 800)
position_output = jnt_traj.eval(ts_sample)
velocity_output = jnt_traj.evald(ts_sample)
acceleration_output = jnt_traj.evaldd(ts_sample)
# --------------------------- plotting the algorithm output ---------------------------
# plt.subplot(3,1,1)
# plt.plot(ts_sample, position_output)
# plt.subplot(3,1,2)
# plt.plot(ts_sample, velocity_output)
# plt.subplot(3,1,3)
# plt.plot(ts_sample, acceleration_output)
# plt.show()
# --------------------------- end plot the algorithm output ---------------------------
start_time = rospy.get_time()
for i in range(len(ts_sample) - 1):
JointCommandMessage.position = position_output[i]
JointCommandMessage.velocity = velocity_output[i]
JointCommandMessage.acceleration = acceleration_output[i]
desire_time = ts_sample[i + 1]
while (rospy.get_time() - start_time) < desire_time:
trajectory_publisher.publish(JointCommandMessage)
JointCommandMessage.position = position_output[-1]
JointCommandMessage.velocity = velocity_output[-1]
JointCommandMessage.acceleration = acceleration_output[-1]
trajectory_publisher.publish(JointCommandMessage)
def enable2nextcycle(freq, time):
en = True
if 2 * np.pi / freq < time:
en = False
return en
if __name__ == '__main__':
rospy.init_node('motion_f_const')
rate = rospy.Rate(500)
robot = robot_control()
time.sleep(1)
command = JointCommand()
command.mode = 1
command.names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5',
'right_j6'
]
# Variables
NUM_DATA = 500
joint1_en = False
joint2_en = False
joint3_en = False
joint4_en = True
joint5_en = False
joint6_en = False
joint7_en = False
def main():
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('avalos_limb_py', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = 'right_arm'
group = moveit_commander.MoveGroupCommander(group_name)
#frecuency for Sawyer robot
f = 100
alfa = 0.9
rate = rospy.Rate(f)
# add gripper
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
gripper.open()
moveit_robot_state = RobotState()
joint_state = JointState()
joint_state.header = Header()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
limb = Limb()
limb.move_to_neutral()
limb.move_to_joint_positions({"right_j6": 2})
group.clear_pose_targets()
group.set_start_state_to_current_state()
# We can get the joint values from the group and adjust some of the values:
pose_goal = geometry_msgs.msg.Pose()
# Orientation
pose_goal.orientation.x = -1
pose_goal.orientation.y = 0.0
pose_goal.orientation.z = 0.0
pose_goal.orientation.w = 0.0
q0 = np.array([])
q1 = np.array([])
q2 = np.array([])
q3 = np.array([])
q4 = np.array([])
q5 = np.array([])
q6 = np.array([])
# Cartesian position - Carga '01'
pose_goal.position.x = 0.758552
pose_goal.position.y = -0.3435
pose_goal.position.z = 0.25
group.set_pose_target(pose_goal)
carga1 = group.plan().joint_trajectory.points
n = len(carga1)
joint_state.position = [
carga1[-1].positions[0], carga1[-1].positions[1],
carga1[-1].positions[2], carga1[-1].positions[3],
carga1[-1].positions[4], carga1[-1].positions[5],
carga1[-1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
tmp = np.array([])
if (n > 8):
tmp = np.append(tmp, 0)
k = int(math.sqrt(n) + 2)
r = int((n - 1) / float(k))
for i in range(k):
print i
tmp = np.append(tmp, int(r * (i + 1) - 1))
tmp = np.append(tmp, n - 1)
else:
for i in range(n):
print i
tmp = np.append(tmp, i)
print "tmp:", tmp
for i in range(len(tmp)):
q0 = np.append(q0, carga1[int(tmp[i])].positions[0])
q1 = np.append(q1, carga1[int(tmp[i])].positions[1])
q2 = np.append(q2, carga1[int(tmp[i])].positions[2])
q3 = np.append(q3, carga1[int(tmp[i])].positions[3])
q4 = np.append(q4, carga1[int(tmp[i])].positions[4])
q5 = np.append(q5, carga1[int(tmp[i])].positions[5])
q6 = np.append(q6, carga1[int(tmp[i])].positions[6])
print "q000", q0
# Cartesian position - Carga '00'
#pose_goal.position.x = 0.85
#pose_goal.position.y = -0.4
pose_goal.position.z = -0.01
group.set_pose_target(pose_goal)
carga0 = group.plan().joint_trajectory.points
q0 = np.append(q0, carga0[-1].positions[0])
q1 = np.append(q1, carga0[-1].positions[1])
q2 = np.append(q2, carga0[-1].positions[2])
q3 = np.append(q3, carga0[-1].positions[3])
q4 = np.append(q4, carga0[-1].positions[4])
q5 = np.append(q5, carga0[-1].positions[5])
q6 = np.append(q6, carga0[-1].positions[6])
#'''
q = np.array([q0, q1, q2, q3, q4, q5, q6])
print "q001", q0
m_time, t_min_tiempo = min_time(q)
start = time.time()
opt = Opt_avalos(q, f, 0.9)
v_time = opt.full_time()
j_1, v_1, a_1, jk_1 = generate_path_cub(q, v_time, f)
ext_1 = len(j_1[0, :])
end = time.time()
print('Process Time:', end - start)
v_jk = sqrt(np.mean(np.square(jk_1)))
print("Opt Time:", v_time)
print("Min Time:", m_time)
#print('Optimizacion:',opt.result())
max_v = np.amax(np.absolute(v_1))
max_ac = np.amax(np.absolute(a_1))
max_jk = np.amax(np.absolute(jk_1))
print "Max Velo:", max_v
print "Max Acel:", max_ac
print "Max Jerk:", max_jk
#raw_input('Iniciar_CT_execute?')
#Build message
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5"
] #,"right_j6"]
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
q0 = np.array([])
q1 = np.array([])
q2 = np.array([])
q3 = np.array([])
q4 = np.array([])
q5 = np.array([])
q6 = np.array([])
q0 = np.append(q0, carga0[-1].positions[0])
q1 = np.append(q1, carga0[-1].positions[1])
q2 = np.append(q2, carga0[-1].positions[2])
q3 = np.append(q3, carga0[-1].positions[3])
q4 = np.append(q4, carga0[-1].positions[4])
q5 = np.append(q5, carga0[-1].positions[5])
q6 = np.append(q6, carga0[-1].positions[6])
joint_state.position = [
carga1[-1].positions[0], carga1[-1].positions[1],
carga1[-1].positions[2], carga1[-1].positions[3],
carga1[-1].positions[4], carga1[-1].positions[5],
carga1[-1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
# Cartesian position - Base '01'
pose_goal.position.x = 0.80791
pose_goal.position.y = 0.4461
pose_goal.position.z = 0.2501
group.set_pose_target(pose_goal)
base1 = group.plan().joint_trajectory.points
n = len(base1)
joint_state.position = [
base1[-1].positions[0], base1[-1].positions[1],
base1[-1].positions[2], base1[-1].positions[3],
base1[-1].positions[4], base1[-1].positions[5],
base1[-1].positions[6]
]
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)
tmp = np.array([])
if (n > 14):
tmp = np.append(tmp, 0)
k = int(math.sqrt(n) + 3)
r = int((n - 1) / float(k))
for i in range(k):
print i
tmp = np.append(tmp, int(r * (i + 1) - 1))
tmp = np.append(tmp, n - 1)
else:
for i in range(n):
print i
tmp = np.append(tmp, i)
print "tmp:", tmp
for i in range(len(tmp)):
q0 = np.append(q0, base1[int(tmp[i])].positions[0])
q1 = np.append(q1, base1[int(tmp[i])].positions[1])
q2 = np.append(q2, base1[int(tmp[i])].positions[2])
q3 = np.append(q3, base1[int(tmp[i])].positions[3])
q4 = np.append(q4, base1[int(tmp[i])].positions[4])
q5 = np.append(q5, base1[int(tmp[i])].positions[5])
q6 = np.append(q6, base1[int(tmp[i])].positions[6])
print "q000", q0
# Cartesian position - Base '00'
#pose_goal.position.x = 0.90
#pose_goal.position.y = 0.38
pose_goal.position.z = 0.01
group.set_pose_target(pose_goal)
base0 = group.plan().joint_trajectory.points
q0 = np.append(q0, base0[-1].positions[0])
q1 = np.append(q1, base0[-1].positions[1])
q2 = np.append(q2, base0[-1].positions[2])
q3 = np.append(q3, base0[-1].positions[3])
q4 = np.append(q4, base0[-1].positions[4])
q5 = np.append(q5, base0[-1].positions[5])
q6 = np.append(q6, base0[-1].positions[6])
q = np.array([q0, q1, q2, q3, q4, q5, q6])
print "q001", q0
#print q[0]
#return 0
m_time, t_min_tiempo = min_time(q)
start = time.time()
opt = Opt_avalos(q, f, alfa)
v_time = opt.full_time()
j_2, v_2, a_2, jk_2 = generate_path_cub(q, v_time, f)
ext_2 = len(j_2[0, :])
end = time.time()
print('Process Time:', end - start)
#save_matrix(j,"data_p.txt",f)
#save_matrix(v,"data_v.txt",f)
#save_matrix(a,"data_a.txt",f)
#save_matrix(jk,"data_y.txt",f)
v_jk = sqrt(np.mean(np.square(jk_2)))
print("Opt Time:", v_time)
print("Min Time:", m_time)
#print('Optimizacion:',opt.result())
max_v = np.amax(np.absolute(v_2))
max_ac = np.amax(np.absolute(a_2))
max_jk = np.amax(np.absolute(jk_2))
print "Max Velo:", max_v
print "Max Acel:", max_ac
print "Max Jerk:", max_jk
q0 = np.array([])
q1 = np.array([])
q2 = np.array([])
q3 = np.array([])
q4 = np.array([])
q5 = np.array([])
q6 = np.array([])
q0 = np.append(q0, base0[-1].positions[0])
q1 = np.append(q1, base0[-1].positions[1])
q2 = np.append(q2, base0[-1].positions[2])
q3 = np.append(q3, base0[-1].positions[3])
q4 = np.append(q4, base0[-1].positions[4])
q5 = np.append(q5, base0[-1].positions[5])
q6 = np.append(q6, base0[-1].positions[6])
# Cartesian position - Carga '01'
pose_goal.position.x = 0.7708552
pose_goal.position.y = -0.394135
pose_goal.position.z = 0.24
group.set_pose_target(pose_goal)
carga1 = group.plan().joint_trajectory.points
n = len(carga1)
tmp = np.array([])
if (n > 10):
tmp = np.append(tmp, 0)
k = int(math.sqrt(n) + 2)
r = int((n - 1) / float(k))
for i in range(k):
print i
tmp = np.append(tmp, int(r * (i + 1) - 1))
tmp = np.append(tmp, n - 1)
else:
for i in range(n):
print i
tmp = np.append(tmp, i)
print "tmp:", tmp
for i in range(len(tmp)):
q0 = np.append(q0, carga1[int(tmp[i])].positions[0])
q1 = np.append(q1, carga1[int(tmp[i])].positions[1])
q2 = np.append(q2, carga1[int(tmp[i])].positions[2])
q3 = np.append(q3, carga1[int(tmp[i])].positions[3])
q4 = np.append(q4, carga1[int(tmp[i])].positions[4])
q5 = np.append(q5, carga1[int(tmp[i])].positions[5])
q6 = np.append(q6, carga1[int(tmp[i])].positions[6])
q = np.array([q0, q1, q2, q3, q4, q5, q6])
print "q001", q0
#print q[0]
#return 0
m_time, t_min_tiempo = min_time(q)
start = time.time()
opt = Opt_avalos(q, f, 0.8)
v_time = opt.full_time()
j_3, v_3, a_3, jk_3 = generate_path_cub(q, v_time, f)
ext_3 = len(j_3[0, :])
end = time.time()
print('Process Time:', end - start)
#save_matrix(j,"data_p.txt",f)
#save_matrix(v,"data_v.txt",f)
#save_matrix(a,"data_a.txt",f)
#save_matrix(jk,"data_y.txt",f)
v_jk = sqrt(np.mean(np.square(jk_3)))
print("Opt Time:", v_time)
print("Min Time:", m_time)
#print('Optimizacion:',opt.result())
max_v = np.amax(np.absolute(v_3))
max_ac = np.amax(np.absolute(a_3))
max_jk = np.amax(np.absolute(jk_3))
print "Max Velo:", max_v
print "Max Acel:", max_ac
print "Max Jerk:", max_jk
raw_input('Iniciar_CT_execute?')
#Build message
for n in xrange(ext_1):
my_msg.position = [
j_1[0][n], j_1[1][n], j_1[2][n], j_1[3][n], j_1[4][n],
j_1[5][n]
] #,j_1[6][n]]
my_msg.velocity = [
v_1[0][n], v_1[1][n], v_1[2][n], v_1[3][n], v_1[4][n],
v_1[5][n]
] #,v_1[6][n]]
my_msg.acceleration = [
a_1[0][n], a_1[1][n], a_1[2][n], a_1[3][n], a_1[4][n],
a_1[5][n]
] #,a_1[6][n]]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
time.sleep(0.25)
gripper.close()
print("Control por trayectoria iniciado.")
#time.sleep(0.25)
for n in xrange(ext_2):
my_msg.position = [
j_2[0][n], j_2[1][n], j_2[2][n], j_2[3][n], j_2[4][n],
j_2[5][n]
] #,j_2[6][n]]
my_msg.velocity = [
v_2[0][n], v_2[1][n], v_2[2][n], v_2[3][n], v_2[4][n],
v_2[5][n]
] #,v_2[6][n]]
my_msg.acceleration = [
a_2[0][n], a_2[1][n], a_2[2][n], a_2[3][n], a_2[4][n],
a_2[5][n]
] #,a_2[6][n]]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
gripper.open()
time.sleep(0.5)
#data.writeoff()
print("Programa terminado.")
for n in xrange(ext_3):
my_msg.position = [
j_3[0][n], j_3[1][n], j_3[2][n], j_3[3][n], j_3[4][n],
j_3[5][n]
] #,j_3[6][n]]
my_msg.velocity = [
v_3[0][n], v_3[1][n], v_3[2][n], v_3[3][n], v_3[4][n],
v_3[5][n]
] #,v_3[6][n]]
my_msg.acceleration = [
a_3[0][n], a_3[1][n], a_3[2][n], a_3[3][n], a_3[4][n],
a_3[5][n]
] #,a_3[6][n]]
pub.publish(my_msg)
rate.sleep()
print("Control por trayectoria terminado.")
gripper.open()
#data.writeoff()
print("Programa terminado.")
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
self.pub_joints.publish(_robot)
time.sleep(0.01)
def effort_sinpercycle(start_time):
joint_torq_desired = 0.1 #0.001*np.sin(freq*(time.time() - start_time))# + np.random.randn(1)*0.1
return joint_torq_desired
if __name__ == '__main__':
rospy.init_node('motion_f_const')
rate = rospy.Rate(100)
robot = robot_control()
time.sleep(1)
command = JointCommand()
command.mode = 3
command.names = ['right_j3']
#model = load_model('model_zero.h5')
# Variables
NUM_DATA = 2000
joint1_en = False
joint2_en = False
joint3_en = False
joint4_en = True
joint5_en = False
joint6_en = False
joint7_en = False
amplitude = 0.1
DIRECTORY = '../data/sawyer/temp/'
FILENAME = DIRECTORY + 'torque_test.csv'
def __init__(self, mode="real"):
"""
Arguments
mode - "real" for the real robot (default), "sim" for the gazebo
simulation
"""
# Lengths for the links (in meters)
self.l0 = 0.081; self.h = 0.317; self.l1 = 0.1925; self.l2 = 0.4;
self.l3 = 0.1685; self.l4 = 0.4; self.l5 = 0.1363; self.l6 = 0.13375
# Memory allocation
self.Jp = np.zeros((3,7))
self.J = np.zeros((7,7))
# Joint limits
self.qmin = (-3.05, -3.81, -3.04, -3.04, -2.98, -2.98, -4.71)
self.qmax = ( 3.05, 2.27, 3.04, 3.04, 2.98, 2.98, 4.71)
# Topic that contains the sensed joint configuration
rospy.Subscriber("/robot/joint_states", JointState, self._readJoints)
# Sensed joint configuration
self.jstate_ = 7*[0.,]
# Markers for current and desired pose/position of the wrist
self.wrist_current_frame = FrameMarker()
self.wrist_des_frame = FrameMarker(0.5)
self.wrist_current_ball = BallMarker(color['GREEN'])
self.wrist_des_ball = BallMarker(color['RED'])
# Initialize the markers at zero
x0 = np.array([0., 0., 0., 1., 0., 0., 0.])
self.wrist_current_frame.setPose(x0)
self.wrist_des_frame.setPose(x0)
# Store the mode
self.mode = mode
# For the real robot
if (self.mode=="real"):
ns = "/robot/limb/right/"
jtopic = ns + "joint_command"
self.pubjs = rospy.Publisher(jtopic, JointCommand, queue_size=10)
self.jcommand = JointCommand()
self.jcommand.mode = 1
self.jcommand.names = ["right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"]
self.set_joints = self._publish_real
# For the Gazebo simulation
elif (self.mode=="sim"):
ns = "/robot/right_joint_position_controller/joints"
topic0 = ns + "/right_j0_controller/command"
topic1 = ns + "/right_j1_controller/command"
topic2 = ns + "/right_j2_controller/command"
topic3 = ns + "/right_j3_controller/command"
topic4 = ns + "/right_j4_controller/command"
topic5 = ns + "/right_j5_controller/command"
topic6 = ns + "/right_j6_controller/command"
self.pub0 = rospy.Publisher(topic0, Float64, queue_size=10)
self.pub1 = rospy.Publisher(topic1, Float64, queue_size=10)
self.pub2 = rospy.Publisher(topic2, Float64, queue_size=10)
self.pub3 = rospy.Publisher(topic3, Float64, queue_size=10)
self.pub4 = rospy.Publisher(topic4, Float64, queue_size=10)
self.pub5 = rospy.Publisher(topic5, Float64, queue_size=10)
self.pub6 = rospy.Publisher(topic6, Float64, queue_size=10)
self.set_joints = self._publish_sim
else:
rospy.logerr("Joint Interface: invalid option for mode")
def __init__(self, limb="right", synchronous_pub=False):
"""
Constructor.
@type limb: str
@param limb: limb to interface
@type synchronous_pub: bool
@param synchronous_pub: designates the JointCommand Publisher
as Synchronous if True and Asynchronous if False.
Synchronous Publishing means that all joint_commands publishing to
the robot's joints will block until the message has been serialized
into a buffer and that buffer has been written to the transport
of every current Subscriber. This yields predicable and consistent
timing of messages being delivered from this Publisher. However,
when using this mode, it is possible for a blocking Subscriber to
prevent the joint_command functions from exiting. Unless you need exact
JointCommand timing, default to Asynchronous Publishing (False).
For more information about Synchronous Publishing see:
http://wiki.ros.org/rospy/Overview/Publishers%20and%20Subscribers#queue_size:_publish.28.29_behavior_and_queuing
"""
params = RobotParams()
limb_names = params.get_limb_names()
if limb not in limb_names:
rospy.logerr("Cannot detect limb {0} on this robot."
" Valid limbs are {1}. Exiting Limb.init().".format(
limb, limb_names))
return
joint_names = params.get_joint_names(limb)
if not joint_names:
rospy.logerr("Cannot detect joint names for limb {0} on this "
"robot. Exiting Limb.init().".format(limb))
return
self.name = limb
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._joint_names = joint_names
self._collision_state = False
self._tip_states = None
ns = '/robot/limb/' + limb + '/'
self._command_msg = JointCommand()
self._pub_speed_ratio = rospy.Publisher(
ns + 'set_speed_ratio',
Float64,
latch=True,
queue_size=10)
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._pub_joint_cmd = rospy.Publisher(
ns + 'joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
ns + 'joint_command_timeout',
Float64,
latch=True,
queue_size=10)
_cartesian_state_sub = rospy.Subscriber(
ns + 'endpoint_state',
EndpointState,
self._on_endpoint_states,
queue_size=1,
tcp_nodelay=True)
_tip_states_sub = rospy.Subscriber(
ns + 'tip_states',
EndpointStates,
self._on_tip_states,
queue_size=1,
tcp_nodelay=True)
_collision_state_sub = rospy.Subscriber(
ns + 'collision_detection_state',
CollisionDetectionState,
self._on_collision_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = '/robot/joint_states'
_joint_state_sub = rospy.Subscriber(
joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
ns_pkn = "ExternalTools/" + limb + "/PositionKinematicsNode/"
self._iksvc = rospy.ServiceProxy(ns_pkn + 'IKService', SolvePositionIK)
self._fksvc = rospy.ServiceProxy(ns_pkn + 'FKService', SolvePositionFK)
rospy.wait_for_service(ns_pkn + 'IKService', 5.0)
rospy.wait_for_service(ns_pkn + 'FKService', 5.0)
err_msg = ("%s limb init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
intera_dataflow.wait_for(lambda: len(list(self._joint_angle.keys())) > 0,
timeout_msg=err_msg, timeout=5.0)
err_msg = ("%s limb init failed to get current endpoint_state "
"from %s") % (self.name.capitalize(), ns + 'endpoint_state')
intera_dataflow.wait_for(lambda: len(list(self._cartesian_pose.keys())) > 0,
timeout_msg=err_msg, timeout=5.0)
err_msg = ("%s limb init failed to get current tip_states "
"from %s") % (self.name.capitalize(), ns + 'tip_states')
intera_dataflow.wait_for(lambda: self._tip_states is not None,
timeout_msg=err_msg, timeout=5.0)
def main():
try:
rospy.init_node('avalos_limb_py')
#Frecuency for Sawyer robot
f = 100
rate = rospy.Rate(f)
#Define topic
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
# Class to record
data = Data()
#Class limb to acces information sawyer
limb = Limb()
#Initial position
limb.move_to_neutral()
time.sleep(3)
# Position init
initial = limb.joint_angles()
print "Posicion inicial terminada"
#begin to record data
data.writeon("cin_directa.txt")
time.sleep(0.5)
limb.move_to_joint_positions({
"right_j6": 0.0,
"right_j5": 0.0,
"right_j4": 0.0,
"right_j3": 0.0,
"right_j2": 0.0,
"right_j1": 0.0,
"right_j0": 0.0
})
limb.move_to_joint_positions({
"right_j6": initial["right_j6"],
"right_j5": initial["right_j5"],
"right_j4": initial["right_j4"],
"right_j3": initial["right_j3"],
"right_j2": initial["right_j2"],
"right_j1": initial["right_j1"],
"right_j0": initial["right_j0"]
})
time.sleep(1)
data.writeoff()
print "FINISH"
initial = limb.joint_angles()
p0 = np.array([
initial["right_j0"], initial["right_j1"], initial["right_j2"],
initial["right_j3"], initial["right_j4"], initial["right_j5"],
initial["right_j6"]
])
# Posiition end
p1 = [0, 0, 0, 0, 0, 0, 0]
p2 = p0
# Knost vector time. We assum the process will take 10 sec
k = np.array([0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1])
tiempo_estimado = 7
k_t = tiempo_estimado * k
k_pt = np.array([k_t[0], k_t[5], k_t[-1]])
# Set inperpolation in linear form
k_j0 = sp.interpolate.interp1d(k_pt, [p0[0], p1[0], p2[0]],
kind='linear')(k_t)
k_j1 = sp.interpolate.interp1d(k_pt, [p0[1], p1[1], p2[1]],
kind='linear')(k_t)
k_j2 = sp.interpolate.interp1d(k_pt, [p0[2], p1[2], p2[2]],
kind='linear')(k_t)
k_j3 = sp.interpolate.interp1d(k_pt, [p0[3], p1[3], p2[3]],
kind='linear')(k_t)
k_j4 = sp.interpolate.interp1d(k_pt, [p0[4], p1[4], p2[4]],
kind='linear')(k_t)
k_j5 = sp.interpolate.interp1d(k_pt, [p0[5], p1[5], p2[5]],
kind='linear')(k_t)
k_j6 = sp.interpolate.interp1d(k_pt, [p0[6], p1[6], p2[6]],
kind='linear')(k_t)
# Obtain all point following the interpolated points
j0 = path_simple_cub_v0(k_j0, k_t, f)
j1 = path_simple_cub_v0(k_j1, k_t, f)
j2 = path_simple_cub_v0(k_j2, k_t, f)
j3 = path_simple_cub_v0(k_j3, k_t, f)
j4 = path_simple_cub_v0(k_j4, k_t, f)
j5 = path_simple_cub_v0(k_j5, k_t, f)
j6 = path_simple_cub_v0(k_j6, k_t, f)
l = len(j0)
print "l"
print l
# Vector for velocity
v_j0 = np.zeros(l)
v_j1 = np.zeros(l)
v_j2 = np.zeros(l)
v_j3 = np.zeros(l)
v_j4 = np.zeros(l)
v_j5 = np.zeros(l)
v_j6 = np.zeros(l)
# Vector for acceleration
a_j0 = np.zeros(l)
a_j1 = np.zeros(l)
a_j2 = np.zeros(l)
a_j3 = np.zeros(l)
a_j4 = np.zeros(l)
a_j5 = np.zeros(l)
a_j6 = np.zeros(l)
# Vector for jerk
jk_j0 = np.zeros(l)
jk_j1 = np.zeros(l)
jk_j2 = np.zeros(l)
jk_j3 = np.zeros(l)
jk_j4 = np.zeros(l)
jk_j5 = np.zeros(l)
jk_j6 = np.zeros(l)
for i in xrange(l - 1):
v_j0[i] = (j0[i + 1] - j0[i]) * f
v_j1[i] = (j1[i + 1] - j1[i]) * f
v_j2[i] = (j2[i + 1] - j2[i]) * f
v_j3[i] = (j3[i + 1] - j3[i]) * f
v_j4[i] = (j4[i + 1] - j4[i]) * f
v_j5[i] = (j5[i + 1] - j5[i]) * f
v_j6[i] = (j6[i + 1] - j6[i]) * f
v_j0[-1] = v_j0[-2]
v_j1[-1] = v_j0[-2]
v_j2[-1] = v_j0[-2]
v_j3[-1] = v_j0[-2]
v_j4[-1] = v_j0[-2]
v_j5[-1] = v_j0[-2]
v_j6[-1] = v_j0[-2]
for i in xrange(l - 1):
a_j0[i] = (v_j0[i + 1] - v_j0[i]) * f
a_j1[i] = (v_j1[i + 1] - v_j1[i]) * f
a_j2[i] = (v_j2[i + 1] - v_j2[i]) * f
a_j3[i] = (v_j3[i + 1] - v_j3[i]) * f
a_j4[i] = (v_j4[i + 1] - v_j4[i]) * f
a_j5[i] = (v_j5[i + 1] - v_j5[i]) * f
a_j6[i] = (v_j6[i + 1] - v_j6[i]) * f
a_j0[-2] = a_j0[-3]
a_j1[-2] = a_j1[-3]
a_j2[-2] = a_j2[-3]
a_j3[-2] = a_j3[-3]
a_j4[-2] = a_j4[-3]
a_j5[-2] = a_j5[-3]
a_j6[-2] = a_j6[-3]
a_j0[-1] = a_j0[-2]
a_j1[-1] = a_j1[-2]
a_j2[-1] = a_j2[-2]
a_j3[-1] = a_j3[-2]
a_j4[-1] = a_j4[-2]
a_j5[-1] = a_j5[-2]
a_j6[-1] = a_j6[-2]
for i in xrange(l - 1):
jk_j0[i] = (a_j0[i + 1] - a_j0[i]) * f
jk_j1[i] = (a_j1[i + 1] - a_j1[i]) * f
jk_j2[i] = (a_j2[i + 1] - a_j2[i]) * f
jk_j3[i] = (a_j3[i + 1] - a_j3[i]) * f
jk_j4[i] = (a_j4[i + 1] - a_j4[i]) * f
jk_j5[i] = (a_j5[i + 1] - a_j5[i]) * f
jk_j6[i] = (a_j6[i + 1] - a_j6[i]) * f
jk_j0[-3] = jk_j0[-4]
jk_j1[-3] = jk_j1[-4]
jk_j2[-3] = jk_j2[-4]
jk_j3[-3] = jk_j3[-4]
jk_j4[-3] = jk_j4[-4]
jk_j5[-3] = jk_j5[-4]
jk_j6[-3] = jk_j6[-4]
jk_j0[-2] = jk_j0[-3]
jk_j1[-2] = jk_j1[-3]
jk_j2[-2] = jk_j2[-3]
jk_j3[-2] = jk_j3[-3]
jk_j4[-2] = jk_j4[-3]
jk_j5[-2] = jk_j5[-3]
jk_j6[-2] = jk_j6[-3]
jk_j0[-1] = jk_j0[-2]
jk_j1[-1] = jk_j1[-2]
jk_j2[-1] = jk_j2[-2]
jk_j3[-1] = jk_j3[-2]
jk_j4[-1] = jk_j4[-2]
jk_j5[-1] = jk_j5[-2]
jk_j6[-1] = jk_j6[-2]
j = np.array([j0, j1, j2, j3, j4, j5, j6])
v = np.array([v_j0, v_j1, v_j2, v_j3, v_j4, v_j5, v_j6])
a = np.array([a_j0, a_j1, a_j2, a_j3, a_j4, a_j5, a_j6])
jk = np.array([jk_j0, jk_j1, jk_j2, jk_j3, jk_j4, jk_j5, jk_j6])
save_matrix(j, "data_p.txt", f)
save_matrix(v, "data_v.txt", f)
save_matrix(a, "data_a.txt", f)
save_matrix(jk, "data_y.txt", f)
#Build message
my_msg = JointCommand()
# POSITION_MODE
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4",
"right_j5", "right_j6"
]
data.writeon("cin_trayectoria.txt")
time.sleep(0.5)
for i in xrange(l):
my_msg.position = [j0[i], j1[i], j2[i], j3[i], j4[i], j5[i], j6[i]]
my_msg.velocity = [
v_j0[i], v_j1[i], v_j2[i], v_j3[i], v_j4[i], v_j5[i], v_j6[i]
]
my_msg.acceleration = [
a_j0[i], a_j1[i], a_j2[i], a_j3[i], a_j4[i], a_j5[i], a_j6[i]
]
pub.publish(my_msg)
rate.sleep()
time.sleep(1)
data.writeoff()
print "Programa terminado "
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user.')
def move_to_ja(self, waypoints, duration=1.5):
"""
:param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point.
Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint
:param duration: Total time trajectory will take before ending
"""
self._try_enable()
jointnames = self._limb.joint_names()
prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames])
waypoints = np.array([prev_joint] + waypoints)
spline = CSpline(waypoints, duration)
start_time = rospy.get_time() # in seconds
finish_time = start_time + duration # in seconds
time = rospy.get_time()
while time < finish_time:
pos, velocity, acceleration = spline.get(time - start_time)
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.position = pos
command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag)
self._cmd_publisher.publish(command)
self._control_rate.sleep()
time = rospy.get_time()
for i in xrange(10):
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.position = waypoints[-1]
self._cmd_publisher.publish(command)
self._control_rate.sleep()
def set_move():
F = 100 #Frecuencia de envio
rate = rospy.Rate(F) # hz
limb = intera_interface.Limb('right')
limb.move_to_neutral()
ik_service_client_full([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0])
print "Posicion neutral terminada"
time.sleep(1)
p1 = np.array([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0])
p2 = np.array([0.662, 0.200, 0.290, 0.717, 0, 0.717, 0])
p3 = np.array([0.662, -0.150, 0.290, 0.717, 0, 0.717, 0])
p4 = np.array([0.662, -0.450, -0.040, 0.717, 0, 0.717, 0])
p5 = np.array([0.662, -0.150, 0.290, 0.717, 0, 0.717, 0])
p6 = np.array([0.662, 0.200, 0.290, 0.717, 0, 0.717, 0])
p7 = np.array([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0])
data = Data()
data.writeon("IK_data.txt")
# Control de posicion por cinematica inversa
time.sleep(0.75)
[succes, q1] = ik_service_client_full(p1)
[succes, q2] = ik_service_client_full(p2)
[succes, q3] = ik_service_client_full(p3)
[succes, q4] = ik_service_client_full(p4)
[succes, q5] = ik_service_client_full(p5)
[succes, q6] = ik_service_client_full(p6)
[succes, q7] = ik_service_client_full(p7)
time.sleep(0.75)
data.writeoff()
print "Control de Posicion Cinematica Inversa terminada"
names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5",
"right_j6"
]
pub = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=10)
# Control de trayectoria cinematica inversa
q = np.matrix([q1, q2, q3, q4, q5, q6, q7])
print q
[t, t_rec] = min_time(q, F)
k = 1.25 #Factor de Tiempo
t_points = [k * x for x in t]
print "t_points", t_points
#print "Tiempo de recorrido",t_rec,"s"
[j, ext] = generate_path_cub(q, t_points, F)
[v, ext] = generate_vel(j, F)
[a, ext] = generate_acel(v, F)
[jk, v_jk, ext] = generate_jerk(a, F)
v_t = 6 * (t_points[-1] - t_points[0])
raw_input('Iniciar?')
print "Valor tiempo: ", v_t, "Valor jerk", v_jk
save_matrix(j, "save_data_p.txt", F)
save_matrix(v, "save_data_v.txt", F)
save_matrix(a, "save_data_a.txt", F)
save_matrix(jk, "save_data_y.txt", F)
ik_service_client_full([0.662, 0.450, -0.040, 0.717, 0, 0.717, 0])
time.sleep(1)
my_msg = JointCommand()
my_msg.mode = 4
my_msg.names = [
"right_j0", "right_j1", "right_j2", "right_j3", "right_j4", "right_j5",
"right_j6"
]
data.writeon("S_data.txt")
time.sleep(0.75)
if (my_msg.mode == 4):
for n in range(ext):
my_msg.position = [
j[0][n], j[1][n], j[2][n], j[3][n], j[4][n], j[5][n], j[6][n]
]
my_msg.velocity = [
v[0][n], v[1][n], v[2][n], v[3][n], v[4][n], v[5][n], v[6][n]
]
my_msg.acceleration = [
a[0][n], a[1][n], a[2][n], a[3][n], a[4][n], a[5][n], a[6][n]
]
pub.publish(my_msg)
rate.sleep()
time.sleep(0.75)
return True
def move_with_impedance(self, waypoints, duration=1.5):
"""
Moves from curent position to final position while hitting waypoints
:param waypoints: List of arrays containing waypoint joint angles
:param duration: trajectory duration
"""
self._try_enable()
jointnames = self.limb.joint_names()
prev_joint = np.array([self.limb.joint_angle(j) for j in jointnames])
waypoints = np.array([prev_joint] + waypoints)
spline = CSpline(waypoints, duration)
start_time = rospy.get_time() # in seconds
finish_time = start_time + duration # in seconds
time = rospy.get_time()
while time < finish_time:
pos, velocity, acceleration = spline.get(time - start_time)
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.position = pos
command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
command.acceleration = np.clip(acceleration, -max_accel_mag,
max_accel_mag)
self._cmd_publisher.publish(command)
self.control_rate.sleep()
time = rospy.get_time()
for i in xrange(10):
command = JointCommand()
command.mode = JointCommand.POSITION_MODE
command.names = jointnames
command.position = waypoints[-1]
self._cmd_publisher.publish(command)
self.control_rate.sleep()
