def calcularFitness(individual, qObjs):
fitness = 0
KP = np.array(individual[0:7])
KD = np.array(individual[7:14])
KI = np.array(individual[14:21])
m = {
'left_w0': 0.0,
'left_w1': 0.0,
'left_w2': 0.0,
'left_e0': 0.0,
'left_e1': 0.0,
'left_s0': -1.0,
'left_s1': 0.0
}
limb.move_to_joint_positions(m)
#Creamos MENSAJE
msg = JointCommand()
#Modo - 1 (CONTROL POR POSICION)
msg.mode = 3
msg.names = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
errores = []
errores2 = []
dt = []
t = 0
i = 0
q_sensores = []
#kinematics = baxter_kinematics('left')
while i < len(qObjs):
dt.append(time.clock())
qDes = qObjs[i]
angles = limb.joint_angles()
q = np.array([
angles['left_s0'], angles['left_s1'], angles['left_e0'],
angles['left_e1'], angles['left_w0'], angles['left_w1'],
angles['left_w2']
])
q_sensores.append(q)
error = np.array(qDes - q)
errores.append(error)
f_ext = np.zeros((6, 1))
f_ext[2] = -9.8
if i != 0:
der = (errores[len(errores) - 1] - errores[len(errores) - 2]) / (
dt[len(dt) - 2] - dt[len(dt) - 1])
torque = KP * error + KD * np.array(der)
else:
der = errores[len(errores) - 1]
torque = KP * error + KD * np.array(der)
#f = applyFext(kinematics,f_ext)
#torque = torque + f
print(torque[0])
m = {
'left_w0': torque[4],
'left_w1': torque[5],
'left_w2': torque[6],
'left_e0': torque[2],
'left_e1': torque[3],
'left_s0': torque[0],
'left_s1': torque[1]
}
msg.command = [
torque[0], torque[1], torque[2], torque[3], torque[4], torque[5],
torque[6]
]
pub.publish(msg) #Publicamos el mensaje
rate.sleep() #Esperamos para conseguir el rate deseado
for l in range(len(error)):
fitness = fitness + error[l] * error[l]
i = i + 10
print(fitness)
return fitness
'/ExternalTools/right/PositionKinematicsNode/IKService')
ikr = rospy.ServiceProxy(
'/ExternalTools/right/PositionKinematicsNode/IKService',
SolvePositionIK)
ikl = rospy.ServiceProxy(
'/ExternalTools/left/PositionKinematicsNode/IKService',
SolvePositionIK)
pubr = rospy.Publisher('/robot/limb/right/joint_command',
JointCommand,
queue_size=1)
publ = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=1)
cmd = JointCommand()
cmd.mode = 1
req = SolvePositionIKRequest()
req.seed_mode = 0
M0 = transformations.quaternion_matrix((0.031, 0.808, .587, 0.028))
M0[0, 3] = 0.7
M0[1, 3] = -0.137
M0[2, 3] = 0.357
Md = np.eye(4)
Mrl = np.zeros((4, 4))
Mrl[2, 3] = 0.2
Mrl[2, 0] = Mrl[0, 2] = -1
def process(self):
if self.currentAssemblyState == None:
#not enabled, return
print "Waiting for /robot/state..."
time.sleep(1.0)
return
if self.currentAssemblyState.stopped:
print "Robot is stopped"
time.sleep(1.0)
return
if self.currentJointStates == None:
print "Waiting for joint states to be published..."
time.sleep(1.0)
return
if self.lastUpdateTime == None:
self.lastUpdateTime = self.currentJointStates.header.stamp
return
self.currentTime = self.currentJointStates.header.stamp
#convert to Klamp't message
klamptInput = dict()
klamptInput['t'] = self.currentTime.to_sec()
klamptInput['dt'] = self.currentTime.to_sec(
) - self.lastUpdateTime.to_sec()
if klamptInput['dt'] == 0.0:
#no new clock messages read
return
klamptInput['q'] = [0.0] * self.robot_model.numLinks()
klamptInput['dq'] = [0.0] * self.robot_model.numLinks()
klamptInput['torque'] = [0.0] * self.robot_model.numDrivers()
for i, n in enumerate(self.currentJointStates.name):
if n not in self.nameToLinkIndex:
if n != 'torso_t0':
print "Ignoring state of link", n
continue
klamptIndex = self.nameToLinkIndex[n]
klamptInput['q'][klamptIndex] = self.currentJointStates.position[i]
if len(self.currentJointStates.velocity) > 0:
klamptInput['dq'][
klamptIndex] = self.currentJointStates.velocity[i]
if len(self.currentJointStates.effort) > 0:
driverIndex = self.nameToDriverIndex[n]
klamptInput['torque'][
driverIndex] = self.currentJointStates.effort[i]
#if self.currentHeadState != None:
# klamptInput['q'][self.head_pan_link_index] = self.currentHeadState.pan
#output is defined in SerialController
klamptReply = self.output(**klamptInput)
if klamptReply == None:
return False
#now conver the Klamp't reply to a Baxter command
lcmd = JointCommand()
rcmd = JointCommand()
lcmd.names = self.baxter_larm_names
rcmd.names = self.baxter_rarm_names
hpcmd = HeadPanCommand()
hncmd = Bool()
if 'qcmd' in klamptReply:
#use position mode
lcmd.mode = rcmd.mode = POSITION_MODE
q = klamptReply['qcmd']
lcmd.command = [q[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [q[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd.target = q[self.head_pan_link_index]
hpcmd.speed = q[self.head_pan_link_index]
elif 'dqcmd' in klamptReply:
lcmd.mode = rcmd.mode = VELOCITY_MODE
dq = klamptReply['dqcmd']
lcmd.command = [dq[self.nameToLinkIndex[n]] for n in lcmd.names]
rcmd.command = [dq[self.nameToLinkIndex[n]] for n in rcmd.names]
hpcmd = None
elif 'torquecmd' in klamptReply:
lcmd.mode = rcmd.mode = TORQUE_MODE
torque = klamptReply['torquecmd']
lcmd.command = [
torque[self.nameToDriverIndex[n]] for n in lcmd.names
]
rcmd.command = [
torque[self.nameToDriverIndex[n]] for n in rcmd.names
]
hpcmd = None
else:
lcmd = rcmd = None
hpcmd = None
hncmd = None
if self.currentAssemblyState.estop_button != AssemblyState.ESTOP_BUTTON_UNPRESSED:
print "Estop is on..."
time.sleep(1.0)
elif not self.currentAssemblyState.enabled:
print "Waiting for robot to turn on..."
time.sleep(1.0)
else:
#publish the messages
if lcmd:
self.pub_larm.publish(lcmd)
if rcmd:
self.pub_rarm.publish(rcmd)
if hpcmd:
self.pub_hpan.publish(hpcmd)
if hncmd:
self.pub_hnod.publish(hncmd)
self.lastUpdateTime = self.currentTime
return True
def main():
rospy.init_node('trayectoria_circular')
#Ponemos RATE de mensaje
rate = rospy.Rate(5)
#Creamos objeto PUBLICADOR
pub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
arg_fmt = argparse.ArgumentDefaultsHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument("-l",
"--limb",
dest="limb",
default="left",
choices=['left', 'right'],
help="joint trajectory limb")
parser.add_argument("-R",
"--radius",
dest="radius",
default=0.2,
type=float,
help="trajectory radius")
parser.add_argument("-v",
"--velocity",
dest="velocity",
default=5.0,
type=float,
help="trajectory velocity")
parser.add_argument("-r",
"--revolution",
dest="revolution",
default=0.9,
type=float,
help="Revolutions")
args = parser.parse_args(rospy.myargv()[1:])
kin = baxter_kinematics(args.limb)
limb = baxter_interface.Limb(args.limb)
timeout = args.velocity / 10.0
radio = args.radius
decremento = 0.001
posiciones = []
centro_y = 0.35
centro_z = 0.1
if args.limb == 'right':
centro_y = centro_y * (-1)
# FK Position
limb.move_to_neutral()
kp = kin.forward_position_kinematics()
#Array donde vamos acumulando configuraciones q1,q2,...,qn para seguir la trayectoria
#Util para control por Torques con PID
posiciones.append(kp)
#Centro de la Circunferencia
pos = [kp[0], kp[1] - centro_y, kp[2] + centro_z]
rot = [kp[3], kp[4], kp[5], kp[6]]
#Calculamos configuracion Qcentro
l = kin.inverse_kinematics(pos, rot)
m = {
args.limb + '_w0': l[4],
args.limb + '_w1': l[5],
args.limb + '_w2': l[6],
args.limb + '_e0': l[2],
args.limb + '_e1': l[3],
args.limb + '_s0': l[0],
args.limb + '_s1': l[1]
}
#Movemos al centro de la circunferencia
limb.move_to_joint_positions(m)
posiciones.append(l)
puntos_circunferencia = []
puntos_circunferencia2 = []
#Calculamos los valores que tomara Z y que nos servira para calcular Y posteriormente
valoresTomar = []
i = 1
while (i >= -0.5):
valoresTomar.append(i)
i = i - decremento
#Calculamos las posiciones para describir circunferencia en plano ZY
for z in valoresTomar:
c = 2 * pos[1]
b = (-1) * (-z * z + 2 * z * pos[2] + radio * radio - pos[2] * pos[2] -
pos[1] * pos[1])
if c * c - 4 * b > 0:
y = (c + sqrt(c * c - 4 * b)) / 2
y2 = (c - sqrt(c * c - 4 * b)) / 2
puntos_circunferencia.append([y, z])
puntos_circunferencia2.append([y2, z])
Posiciones = []
Rotaciones = []
ErroresPos = []
ErroresRot = []
#Vamos PUBLICANDO las posiciones que deseamos alcanzar para seguir la trayectoria
#Creamos MENSAJE
msg = JointCommand()
#Modo - 1 (CONTROL POR POSICION)
msg.mode = 1
msg.names = [
args.limb + '_w0', args.limb + '_w1', args.limb + '_w2',
args.limb + '_e0', args.limb + '_e1', args.limb + '_s0',
args.limb + '_s1'
]
for p in puntos_circunferencia2:
kp = kin.forward_position_kinematics()
#Calculamos (x,y,z)
pos = [kp[0], p[0], p[1]]
#Calculamos tupla Orientacion (i,j,w,k)
rot = [kp[3], kp[4], kp[5], kp[6]]
#Calculamos (theta1,...) para (x,y,z)
l = kin.inverse_kinematics(pos, rot)
#Posicion Valida
if l != None:
Posiciones.append(pos)
Rotaciones.append(rot)
msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]]
pub.publish(msg)
rate.sleep()
ErroresRot.append(rot)
posiciones.append(l)
else:
ErroresPos.append(pos)
puntos_circunferencia = puntos_circunferencia[::-1]
for p in puntos_circunferencia:
kp = kin.forward_position_kinematics()
pos = [kp[0], p[0], p[1]]
rot = [kp[3], kp[4], kp[5], kp[6]]
l = kin.inverse_kinematics(pos, rot)
if l != None:
Posiciones.append(pos)
Rotaciones.append(rot)
msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]]
pub.publish(msg)
rate.sleep()
posiciones.append(l)
else:
ErroresPos.append(pos)
ErroresRot.append(rot)
#Caso de que queramos realizar la trayectoria en mas de una ocasion
n_vueltas = args.revolution
k = 1
while k < n_vueltas:
for i in range(len(Posiciones)):
pos = Posiciones[i]
rot = Rotaciones[i]
l = kin.inverse_kinematics(pos, rot)
msg.command = [l[4], l[5], l[6], l[2], l[3], l[0], l[1]]
pub.publish(msg)
rate.sleep()
k = k + 1
np.save('trayectoriaC', np.array(posiciones))
def callback(joints):
joints=JointCommand()
_joints.mode=joints.mode
_joints.names=copy(joints.names)
_joints.command=copy(joints.command)
def __init__(self, addr, robot_model, robot_name='robot'):
SerialController.__init__(self, addr)
#defined in SerialController
self.robot_model = robot_model
self.baxter_larm_names = [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1',
'left_w2'
]
self.baxter_rarm_names = [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
self.names_klampt_to_baxter = {
'left_upper_shoulder': 'left_s0',
'left_lower_shoulder': 'left_s1',
'left_upper_elbow': 'left_e0',
'left_lower_elbow': 'left_e1',
'left_upper_forearm': 'left_w0',
'left_lower_forearm': 'left_w1',
'left_wrist': 'left_w2',
'right_upper_shoulder': 'right_s0',
'right_lower_shoulder': 'right_s1',
'right_upper_elbow': 'right_e0',
'right_lower_elbow': 'right_e1',
'right_upper_forearm': 'right_w0',
'right_lower_forearm': 'right_w1',
'right_wrist': 'right_w2',
'head': 'head_pan',
'screen': 'head_nod'
}
for l in range(self.robot_model.numLinks()):
klamptName = self.robot_model.link(l).getName()
if klamptName not in self.names_klampt_to_baxter:
self.names_klampt_to_baxter[klamptName] = klamptName
self.baxterDriverNames = [
self.names_klampt_to_baxter[self.robot_model.getDriver(
d).getName()] for d in range(self.robot_model.numDrivers())
]
self.head_pan_link_index = self.robot_model.link("head").index
self.head_nod_link_index = self.robot_model.link("screen").index
self.larm_command = JointCommand()
self.rarm_command = JointCommand()
self.hpan_command = HeadPanCommand()
self.hnod_command = Bool()
# fast indexing structure for partial commands
self.nameToDriverIndex = dict(
zip(self.baxterDriverNames, range(len(self.baxterDriverNames))))
self.nameToLinkIndex = dict(
(self.names_klampt_to_baxter[self.robot_model.link(l).getName()],
l) for l in range(self.robot_model.numLinks()))
# Setup publisher of robot states
self.currentTime = None
self.lastUpdateTime = None
self.currentAssemblyState = None
self.currentJointStates = None
self.currentHeadState = None
rospy.Subscriber("/%s/state" % (robot_name, ), AssemblyState,
self.assemblyStateCallback)
rospy.Subscriber("/%s/joint_states" % (robot_name, ), JointState,
self.jointStatesCallback)
rospy.Subscriber("/%s/head/head_state" % (robot_name, ), HeadState,
self.headStateCallback)
self.pub_larm = rospy.Publisher(
'/%s/limb/left/joint_command' % (robot_name), JointCommand)
self.pub_rarm = rospy.Publisher(
'/%s/limb/right/joint_command' % (robot_name), JointCommand)
self.pub_hpan = rospy.Publisher(
'/%s/head/command_head_pan' % (robot_name), HeadPanCommand)
self.pub_hnod = rospy.Publisher(
'/%s/head/command_head_nod' % (robot_name), Bool)
return
def main():
rospy.init_node('trayectoria_rombo')
#Definimos el RATE de publicacion de mensajes en ROS
rate = rospy.Rate(20)
#Creamos objeto PUBLICADOR
pub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
arg_fmt = argparse.ArgumentDefaultsHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument("-l",
"--limb",
dest="limb",
default="left",
choices=['left', 'right'],
help="joint trajectory limb")
parser.add_argument("-v",
"--velocity",
dest="velocity",
default=5.0,
type=float,
help="trajectory velocity")
#Leemos los argumentos dados como entrada
args = parser.parse_args(rospy.myargv()[1:])
kin = baxter_kinematics(args.limb)
limb = baxter_interface.Limb(args.limb)
timeout = args.velocity / 10.0
n_mov = 32
# Movemos a posicion neutral
limb.move_to_neutral()
# Consultamos la posicion actual en formato (x,y,z) y (i,j,w,k)
kp = kin.forward_position_kinematics()
#Limites del rombo a describir
limite_inferior_y = -0.5
limite_inferior_z = -0.5
proporcionY = limite_inferior_y / n_mov
proporcionZ = limite_inferior_z / n_mov
if args.limb == 'right':
proporcionY = proporcionY * (-1)
#Creamos MENSAJE
msg = JointCommand()
#Modo - 1 (CONTROL POR POSICION)
msg.mode = 1
msg.names = [
args.limb + '_w0', args.limb + '_w1', args.limb + '_w2',
args.limb + '_e0', args.limb + '_e1', args.limb + '_s0',
args.limb + '_s1'
]
#Vamos describiendo la trayectoria de Rombo
for i in range(n_mov):
kp = kin.forward_position_kinematics()
if i < n_mov // 2:
pos = [kp[0], kp[1] + proporcionY, kp[2] - proporcionZ]
else:
pos = [kp[0], kp[1] + proporcionY, kp[2] + proporcionZ]
rot = [kp[3], kp[4], kp[5], kp[6]]
theta_i = kin.inverse_kinematics(pos, rot)
if theta_i != None:
msg.command = [
theta_i[4], theta_i[5], theta_i[6], theta_i[2], theta_i[3],
theta_i[0], theta_i[1]
]
pub.publish(msg) #Publicamos el mensaje
rate.sleep() #Esperamos para conseguir el rate deseado
for i in range(n_mov):
kp = kin.forward_position_kinematics()
if i < n_mov // 2:
pos = [kp[0], kp[1] - proporcionY, kp[2] + proporcionZ]
else:
pos = [kp[0], kp[1] - proporcionY, kp[2] - proporcionZ]
rot = [kp[3], kp[4], kp[5], kp[6]]
theta_i = kin.inverse_kinematics(pos, rot)
if theta_i != None:
msg.command = [
theta_i[4], theta_i[5], theta_i[6], theta_i[2], theta_i[3],
theta_i[0], theta_i[1]
]
pub.publish(msg)
rate.sleep()
## Flags
## Recommended (stable) parameters are: PY_KDL and MOVE_JNT_PSTN
reference_pose_flag = 1 # Used to determine whether to used saved joint angle data for the reference position (true) or not (false).
#Inverse Kinematics Computation
PY_KDL=0
TRAC_IK=1
kinematics_flag=PY_KDL # Used to determine wheter to use kdl or trac_ik for kinematics
# Movement Algorithm
MOVE_JNT_PSTN=0
JOINT_ACT_CLN=1
approach_flag=MOVE_JNT_PSTN # Used to determine whether to use move_to_joint_positions or joint_trajectory_action_client approach
# Globals
_joints = JointCommand()
#------------------------------------ Class ___________ ------------------------------
class Trajectory(object):
def __init__(self, limb):
ns = 'robot/limb/' + limb + '/'
self._client = actionlib.SimpleActionClient( # create simple client w/ topic and msg type
ns + "follow_joint_trajectory",
FollowJointTrajectoryAction,
)
self._goal = FollowJointTrajectoryGoal() # trajectory(header/points);
# path_tolerance
# goal_tolerance
# goal_time_tolerance
self._goal_time_tolerance = rospy.Time(0.1) # Reach goal within this tolerance of final time.
self._goal.goal_time_tolerance = self._goal_time_tolerance # Assign to goal object.
server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0)) # Connect to server within this time.
def main():
rospy.init_node('trayectoria_sinusoidal')
#Ponemos RATE de mensaje
rate = rospy.Rate(1)
#Creamos objeto PUBLICADOR
pub = rospy.Publisher('/robot/limb/left/joint_command',JointCommand,queue_size=10)
arg_fmt = argparse.ArgumentDefaultsHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument(
"-l", "--limb", dest="limb", default="left",
choices=['left', 'right'],
help="joint trajectory limb"
)
parser.add_argument(
"-v", "--velocity", dest="velocity", default=5.0,
type=float, help="trajectory velocity"
)
parser.add_argument(
"-r", "--revolution", dest="revolution", default=10.0,
type=float, help="Revolutions"
)
parser.add_argument(
"-t", "--dt", dest="dt", default=0.025,
type=float, help="Differential T"
)
parser.add_argument(
"-n", "--points", dest="n", default=100,
type=int, help="Number of movements"
)
args = parser.parse_args(rospy.myargv()[1:])
kin = baxter_kinematics(args.limb)
limb = baxter_interface.Limb(args.limb)
timeout = args.velocity/10.0
n_mov = args.n
dt = args.dt
t = 0
#Movemos a posicion neutral
limb.move_to_neutral();
#Creamos MENSAJE
msg = JointCommand()
#Modo - 1 (CONTROL POR POSICION)
msg.mode = 1
msg.names = [args.limb +'_w0',args.limb + '_w1',args.limb +'_w2',args.limb +'_e0',args.limb +'_e1',args.limb +'_s0',args.limb +'_s1']
#Realizamos trayectoria sinusoidal
contador = 0;
while(contador < n_mov):
kp = kin.forward_position_kinematics()
x = 0.5
y = t/100
z = 0.1*sin(3*t)
if(args.limb == 'right'): y = y*(-1);
pos = [kp[0],kp[1]-y,kp[2]-z]
rot = [kp[3],kp[4],kp[5],kp[6]]
theta_i = kin.inverse_kinematics(pos,rot);
if theta_i != None:
msg.command = [theta_i[4],theta_i[5],theta_i[6],theta_i[2],theta_i[3],theta_i[0],theta_i[1]]
pub.publish(msg) #Publicamos el mensaje
rate.sleep() #Esperamos para conseguir el rate deseado
t = t + dt
contador = contador + 1;
def __init__(self):
# Initialize main node for publishing
rospy.init_node('MainAction')
rospy.loginfo('... Initializing MainAction node')
# Enable baxter if it isn't already enabled
baxter_interface.RobotEnable(CHECK_VERSION).state().enabled
baxter_interface.RobotEnable(CHECK_VERSION).enable()
# Get create main publisher for activating the Joint movements
self.pub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
# Create Class to control Baxter's Joints
self.command = JointCommand()
self.rate = rospy.Rate(70)
# Variable to show every print statement for testing
SHOW_RESULTS = False
# Define screen pixels for Baxter's image (face LCD screen)
h = 600
w = 1024
# Load image for the Baxter's screen
img1 = cv2.imread("Tejada.png")
# Resize the image to get to the maximum possible screen size
img1 = cv2.resize(img1, (w, h), interpolation=cv2.INTER_CUBIC)
# Create publisher for the Image that will be loaded to Baxter
pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=1)
# Define message that will be loaded to the Image-publisher
msg1 = cv_bridge.CvBridge().cv2_to_imgmsg(img1, encoding="bgr8")
# Publish the desired message that contains the image
pub.publish(msg1)
# Get main vector for the trajectory
T1 = trajectory_A.TrajectoryA(5)
print("\n... Processing trajectory 1 ...\n")
vector_x = T1.vector_x
vector_y = T1.vector_y
vector_z = T1.vector_z
vector_angle_x = T1.vector_angle_x
vector_angle_y = T1.vector_angle_y
vector_angle_z = T1.vector_angle_z
print("\n... Done processing trajectory 1 ...\n")
# TM_test = transformation.Transformation(vector_angle_x[0], vector_angle_y[0],
# vector_angle_z[0], [vector_x[0], vector_y[0], vector_z[0]])
# print(TM_test.TM)
# THETAS = inverse_kinematics.inverse_kinematics_baxter(TM_test.TM, "left")
# print(THETAS)
# Get each vector of theta_i for all points
self.THETA_1 = []
self.THETA_2 = []
self.THETA_4 = []
self.THETA_5 = []
self.THETA_6 = []
self.THETA_7 = []
# Generate main trajectories finding each angle of the joints with IK
for i in range(len(vector_x)):
# Get each transformation matrix for all cartesian points given
TM_i = transformation.Transformation(
vector_angle_x[i], vector_angle_y[i], vector_angle_z[i],
[vector_x[i], vector_y[i], vector_z[i]])
# Get each THETA_i for each point with the inverse-kinematics
THETAS_i = inverse_kinematics.inverse_kinematics_baxter(
TM_i.TM, "left")
# Create all vectors of THETA_i for all necessary points
self.THETA_1.append(float(THETAS_i[0]))
self.THETA_2.append(float(THETAS_i[1]))
self.THETA_4.append(float(THETAS_i[2]))
self.THETA_5.append(float(THETAS_i[3]))
self.THETA_6.append(float(THETAS_i[4]))
self.THETA_7.append(float(THETAS_i[5]))
if SHOW_RESULTS == True:
print("\n THETA_1: \n", np.rad2deg(self.THETA_1))
print("\n max(THETA_1) =", max(np.rad2deg(self.THETA_1)))
print("\n min(THETA_1) =", min(np.rad2deg(self.THETA_1)))
print("\n THETA_2: \n", np.rad2deg(self.THETA_2))
print("\n max(THETA_2) =", max(np.rad2deg(self.THETA_2)))
print("\n min(THETA_2) =", min(np.rad2deg(self.THETA_2)))
print("\n THETA_4: \n", np.rad2deg(self.THETA_4))
print("\n max(THETA_4) =", max(np.rad2deg(self.THETA_4)))
print("\n min(THETA_4) =", min(np.rad2deg(self.THETA_4)))
print("\n THETA_5: \n", np.rad2deg(self.THETA_5))
print("\n max(THETA_5) =", max(np.rad2deg(self.THETA_5)))
print("\n min(THETA_5) =", min(np.rad2deg(self.THETA_5)))
print("\n THETA_6: \n", np.rad2deg(self.THETA_6))
print("\n max(THETA_6) =", max(np.rad2deg(self.THETA_6)))
print("\n min(THETA_6) =", min(np.rad2deg(self.THETA_6)))
print("\n THETA_7: \n", np.rad2deg(self.THETA_7))
print("\n max(THETA_7) =", max(np.rad2deg(self.THETA_7)))
print("\n min(THETA_7) =", min(np.rad2deg(self.THETA_7)))
def __init__(self, limb):
"""
Constructor.
@type limb: str
@param limb: limb to interface
"""
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 = {
'left': [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
'left_w1', 'left_w2'
],
'right': [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
}
ns = '/robot/limb/' + limb + '/'
self._command_msg = JointCommand()
self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio',
Float64,
latch=True,
queue_size=10)
self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=1)
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)
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)
err_msg = ("%s limb init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg)
#!/usr/bin/env python
from numpy import *
import rospy
from baxter_core_msgs.msg import JointCommand
pub_joints = rospy.Publisher('/robot/limb/right/joint_command', JointCommand)
rospy.init_node('write_to_baxr', anonymous=False)
rate = rospy.Rate(20)
cmd_msg = JointCommand()
cmd_msg.mode = JointCommand.POSITION_MODE
cmd_msg.names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
qini= [0.14035924209151535, -0.9794467330648366,1.588437105855346,0.39154859610775183,-0.8233641878974959, 0.7113835903818606, 0.8348690438066364]
while not rospy.is_shutdown():
cmd_msg.command = qini
pub_joints.publish(cmd_msg)
rate.sleep()
class MovementController(object):
def __init__(self, limb, topic,camera,speedRatio, jointThresholdEnd,
jointThresholdWarning, updateQueryRate, jointFilteringFactorFar,
jointFilteringFactorClose, orientationX, orientationY, orientationZ,
orientationW, timeToWait, name):
# Set Confiuration Variables
self.limb = limb
self.topic=topic
self.camera = camera
self.speedRatio=speedRatio
self.jointThresholdEnd=jointThresholdEnd
self.jointThresholdWarning=jointThresholdWarning
self.updateQueryRate=updateQueryRate
self.jointFilteringFactorFar=jointFilteringFactorFar
self.jointFilteringFactorClose=jointFilteringFactorClose
self.orientationX=orientationX
self.orientationY=orientationY
self.orientationZ=orientationZ
self.orientationW=orientationW
self.timeToWait=timeToWait
# Set variables that are static across executions of the actionServer
self.handDetector = self.createHandDetector()
self.pub_joint_cmd = rospy.Publisher(
'/robot/limb/' + self.limb + '/joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=1)
self.pub_speed_ratio = rospy.Publisher(
'/robot/limb/' + self.limb + '/set_speed_ratio',
Float64,
latch=True,
queue_size=1)
self.pub_speed_ratio.publish(Float64(speedRatio)) # Limit arm speed
self.jointAngles = dict()
self.jointAnglesLock = threading.Lock()
joint_state_topic = 'robot/joint_states'
self.joint_state_sub = rospy.Subscriber(
joint_state_topic,
JointState,
self.onJointStates,
queue_size=1,
tcp_nodelay=True)
self.iksvcStringLimb = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self.iksvcLimb = rospy.ServiceProxy(self.iksvcStringLimb, SolvePositionIK)
# Set Action Server-Related Variables
self.feedback = ReachingHandFeedback()
self.result = ReachingHandResult()
self.actionServer = actionlib.SimpleActionServer(name, ReachingHandAction, execute_cb=self.execute_cb, auto_start = False)
self.actionServer.start()
def createHandDetector(self):
detector = HandDetector(
topic=rospy.get_param("reachingHand/topic"),
rate=rospy.get_param("reachingHand/HandDetector/rate"),
cameraName=self.camera,
handModelPath=rospy.get_param("reachingHand/HandDetector/handModelPath"),
maxDx=rospy.get_param("reachingHand/HandDetector/maxAllowedHandMotion/dx"),
maxDy=rospy.get_param("reachingHand/HandDetector/maxAllowedHandMotion/dy"),
maxDz=rospy.get_param("reachingHand/HandDetector/maxAllowedHandMotion/dz"),
timeToDeleteHand=rospy.get_param("reachingHand/HandDetector/timeToDeleteHand"),
groundDzThreshold=rospy.get_param("reachingHand/HandDetector/groundDzThreshold"),
avgPosDtime=rospy.get_param("reachingHand/HandDetector/avgPosDtime"),
avgHandXYZDtime=rospy.get_param("reachingHand/HandDetector/avgHandXYZDtime"),
maxIterationsWithNoDifference=rospy.get_param("reachingHand/HandDetector/imageDifferenceParams/maxIterations"),
differenceThreshold=rospy.get_param("reachingHand/HandDetector/imageDifferenceParams/differenceThreshold"),
differenceFactor=rospy.get_param("reachingHand/HandDetector/imageDifferenceParams/differenceFactor"),
cascadeScaleFactor=rospy.get_param("reachingHand/HandDetector/cascadeClassifierParams/scale"),
cascadeMinNeighbors=rospy.get_param("reachingHand/HandDetector/cascadeClassifierParams/minNeighbors"),
handHeightIntervalDx=rospy.get_param("reachingHand/HandDetector/handHeightInterval/dx"),
handHeightIntervalDy=rospy.get_param("reachingHand/HandDetector/handHeightInterval/dy"),
getDepthAtMidpointOfHand=rospy.get_param("reachingHand/HandDetector/getDepthAtMidpointOfHand"),
getAveragePos=rospy.get_param("reachingHand/HandDetector/getAveragePos"),
)
return detector
def execute_cb(self, goal):
# Launch the HandDetector
self.handDetector.start()
# Subscribe to necessary topics and create necessary background threads
self.targetPose = None
self.shouldUpdateBaxterTarget = False
self.shouldUpdateBaxterTargetLock = threading.Lock()
self.targetPosition = dict()
self.targetPoint = None
self.killThreads = False
self.targetPositionLock = threading.Lock()
self.targetEndEffectorPointTopic = rospy.Subscriber(self.topic, PointStamped, self.targetEndEffectorPointCallback, queue_size=1)
self.targetEndEffectorPoint = None
self.targetEndEffectorPointLock = threading.Lock()
endEffectorTopic = "robot/limb/"+self.limb+"/endpoint_state"
self.actualEndEffectorPoseTopic = rospy.Subscriber(endEffectorTopic, EndpointState, self.actualEndEffectorPoseCallback, queue_size=1)
self.actualEndEffectorPose = None
self.actualEndEffectorPoseLock = threading.Lock()
endEffectorThread = threading.Thread(target=self.setTargetEndEffectorPosition, args=(self.orientationX, self.orientationY, self.orientationZ, self.orientationW))
endEffectorThread.daemon = True
endEffectorThread.start()
# Move the arm
self.moveArm(self.jointThresholdEnd, self.jointThresholdWarning, self.updateQueryRate, self.jointFilteringFactorFar, self.jointFilteringFactorClose)
# Kill threads, unregister topics, and kill the HandDetector Node
self.killThreads = True
self.targetEndEffectorPointTopic.unregister()
self.actualEndEffectorPoseTopic.unregister()
self.handDetector.stop()
def onJointStates(self, msg):
for i, name in enumerate(msg.name):
# print("onJointStates", i, name)
if self.limb in name:
self.jointAnglesLock.acquire()
self.jointAngles[name] = msg.position[i]
self.jointAnglesLock.release()
def actualEndEffectorPoseCallback(self, state):
if self.actualEndEffectorPoseLock.acquire(False):
self.actualEndEffectorPose = state.pose
self.actualEndEffectorPoseLock.release()
def moveArm(self, jointThresholdEnd, jointThresholdWarning, updateQueryRate,
jointFilteringFactorFar, jointFilteringFactorClose):
rate = rospy.Rate(updateQueryRate)
print("moveArm")
def genf(targetPoint, targetOrientation):
def endEffectorDiff():
self.actualEndEffectorPoseLock.acquire()
if self.actualEndEffectorPose is None:
retValPos = 0
retValOrien = 0
else:
retValPos = math.sqrt((self.actualEndEffectorPose.position.x-targetPoint.x)**2+
(self.actualEndEffectorPose.position.y-targetPoint.y)**2+
(self.actualEndEffectorPose.position.z-targetPoint.z)**2)
retValOrien = math.sqrt((self.actualEndEffectorPose.orientation.x-targetOrientation.x)**2+
(self.actualEndEffectorPose.orientation.y-targetOrientation.y)**2+
(self.actualEndEffectorPose.orientation.z-targetOrientation.z)**2+
(self.actualEndEffectorPose.orientation.w-targetOrientation.w)**2)
# print(joint, retVal)
self.actualEndEffectorPoseLock.release()
return retValPos, retValOrien
return endEffectorDiff
# Wait until we have read the joint state at least once
try:
while not rospy.is_shutdown() and (len(self.jointAngles) == 0 or len(self.targetPosition) == 0 or self.actualEndEffectorPose is None) and not self.killThreads:
if self.actionServer.is_preempt_requested():
self.preempt()
return
rate.sleep()
continue
except KeyboardInterrupt, rospy.ROSInterruptException:
self.abort()
return
reachedTarget = True
commandMsg = JointCommand()
try:
while not rospy.is_shutdown() and not self.killThreads:
if self.actionServer.is_preempt_requested():
self.preempt()
return
if reachedTarget:
self.shouldUpdateBaxterTargetLock.acquire()
if not self.shouldUpdateBaxterTarget:
self.shouldUpdateBaxterTargetLock.release()
rate.sleep()
continue
self.shouldUpdateBaxterTargetLock.release()
# TODO (amal): look into exponential moving average
def filtered_cmd():
# print(self.targetPosition, self.jointAngles)
if not self.closeToTarget:
self.targetPositionLock.acquire()
retPositions = dict()
factor =jointFilteringFactorFar
if self.closeToTarget:
factor = jointFilteringFactorClose
for joint in self.targetPosition.keys():
if self.currentlyMovingTowardsPoint is None or not self.closeToTarget:
self.currentlyMovingTowardsPoint = self.targetPosition
retPositions[joint] = factor * self.currentlyMovingTowardsPoint[joint] + (1-factor) * self.jointAngles[joint] # Get most up to date value
if not self.closeToTarget:
self.targetPositionLock.release()
return retPositions
self.closeToTarget = False
self.currentlyMovingTowardsPoint = None
def loopGuard():
if rospy.is_shutdown() or self.killThreads:
return False
allDiffsWithinWarning = True
# TODO (amal): is this a race condition because I don't lock targetPose?
positionDiff, orientationDiff = genf(self.targetPose.pose.position, self.targetPose.pose.orientation)()
diff = positionDiff+orientationDiff
self.feedback.distance = float(diff)
self.actionServer.publish_feedback(self.feedback)
if diff > jointThresholdWarning:
allDiffsWithinWarning = False
if allDiffsWithinWarning and not self.closeToTarget:
print("WITHIN WARNING!")
# Don't let the target point get changed from now till the hand reaches its target
self.targetPositionLock.acquire()
print("acquired target position lock")
self.closeToTarget = True
positionDiff, orientationDiff = genf(self.targetPose.pose.position, self.targetPose.pose.orientation)()
if positionDiff+orientationDiff > jointThresholdEnd:
print("outside of threshold")
return True
return False
try:
# print("before loop guard")
while loopGuard():
# print("loop guard true")
position = filtered_cmd()
commandMsg.names = position.keys()
commandMsg.command = position.values()
# print(self.jointAngles, self.targetPosition, position)
commandMsg.mode = JointCommand.POSITION_MODE
self.pub_joint_cmd.publish(commandMsg)
print("pre end movememnt", self.closeToTarget)
except KeyboardInterrupt, rospy.ROSInterruptException:
if self.closeToTarget:
self.targetPositionLock.release()
self.closeToTarget = False
self.abort()
return
print("ended movememnt", self.closeToTarget)
if self.closeToTarget:
self.targetPositionLock.release()
self.closeToTarget = False
reachedTarget = True
self.shouldUpdateBaxterTargetLock.acquire()
self.shouldUpdateBaxterTarget = False
print("DONE DONE DONE!!!")
self.shouldUpdateBaxterTargetLock.release()
self.success()
return
except KeyboardInterrupt, rospy.ROSInterruptException:
self.abort()
return
def moveVelocity(self, joint_values_target, speed, thresh, \
forceThresh=float('inf'), maxAcceleration=0.2, is_printing=False):
'''
Move the arm using velocity control to a given list of joint positions.
@type jointTarget: 1x7 vector
@param jointTarget: the given joint configuration
@type speed: real number
@param speed: the speed at which the arm should move
@type thresh: real number
@param thresh: the threshold at which the arm is close enough to the desired joint positions
@type is_printing: bool
@param is_printing: true if debug information should be printed, false otherwise
'''
if not self.isMoving:
return
max_iterations = 10000
step_size = 2.0
oldVelocityCommand = array(self.joint_velocities)
for i in range(0, max_iterations):
joint_error = array(self.joint_values)
joint_velocity_command = joint_error
# calculate joint error, error magnitude, and command velocity
joint_error = joint_values_target - joint_error
joint_velocity_command = joint_error * step_size
error_magnitude = numpy.linalg.norm(joint_error)
joint_velocity_command_magnitude = error_magnitude * step_size
# if command velocity magnitude exceeds speed, scale it back to speed
if joint_velocity_command_magnitude > speed:
joint_velocity_command = speed * joint_velocity_command / joint_velocity_command_magnitude
# scale back if over acceleration limits
acceleration = norm(joint_velocity_command - oldVelocityCommand)
if acceleration > maxAcceleration:
#print("Exceeded max acceleration: {}.".format(acceleration))
a = (maxAcceleration / acceleration)
joint_velocity_command = a * joint_velocity_command + (
1.0 - a) * oldVelocityCommand
oldVelocityCommand = joint_velocity_command
# exit if force magnitude exceeded
if self.forceMagnitude > forceThresh:
print("Force is {}, exceeded threshold {}. Stopping.".format(
self.forceMagnitude, forceThresh))
break
# send velocity command to joints
values_out = [
joint_velocity_command[2], joint_velocity_command[3],
joint_velocity_command[0], joint_velocity_command[1],
joint_velocity_command[4], joint_velocity_command[5],
joint_velocity_command[6]
]
self.pub.publish(
JointCommand(JointCommand.VELOCITY_MODE, values_out,
self.joint_names))
if is_printing:
print "iteration:", i, "error_magnitude: ", error_magnitude
# break loop if close enough to target joint values
if error_magnitude < thresh:
break
rospy.sleep(0.01)
def __init__(self, limb):
"""mZ Constructor.
@type limb: str
@param limb: limb to interface
"""
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.start = False
self.jt = JointTransform()
self.force_cali_mode = True
self._joint_names = {
'left': [
'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0',
'left_w1', 'left_w2'
],
'right': [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
}
ns = '/robot/limb/' + limb + '/'
self.record = open('force.txt', 'w')
self._command_msg = JointCommand()
self._pub_speed_ratio = rospy.Publisher(ns + 'set_speed_ratio',
Float64,
latch=True,
queue_size=10)
self._pub_joint_cmd = rospy.Publisher(ns + 'joint_command',
JointCommand,
tcp_nodelay=True,
queue_size=1)
self._pub_joint_cmd_timeout = rospy.Publisher(ns +
'joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self.x = []
self.y = []
self.step = 0
self.calibrate = False
self.calibrated = False
self.base_force_list = []
self.force_window = []
self.force_thrd = 1.6
_cartesian_state_sub = rospy.Subscriber(ns + 'endpoint_state',
EndpointState,
self._on_endpoint_states,
queue_size=20,
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)
err_msg = ("%s limb init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
baxter_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg)
err_msg = ("%s limb init failed to get current endpoint_state "
"from %s") % (self.name.capitalize(), ns + 'endpoint_state')
baxter_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg)
