def main():
rospy.init_node('armcommand', anonymous=True)
pubPos = rospy.Publisher('/arm_1/arm_controller/position_command', JointPositions, queue_size=10)
pubVel = rospy.Publisher('/arm_1/arm_controller/velocity_command', JointVelocities, queue_size=10)
rospy.sleep(2) #wait for connection
rate = rospy.Rate(70) # 10hz
#initialise positions
jp = JointPositions()
joints = []
for i in range(0, 5):
joints.append(JointValue())
joints[i].joint_uri = "arm_joint_" + str(i+1)
joints[i].unit = "rad"
#initialise velocities
jv = JointVelocities()
vels = []
for i in range(0, 5):
vels.append(JointValue())
vels[i].joint_uri = "arm_joint_" + str(i+1)
vels[i].unit = "s^-1 rad"
#go to home position
for i in range(0,5):
joints[i].value = 0.05
joints[2].value = -joints[2].value
jp.positions = joints;
pubPos.publish(jp)
rospy.sleep(5)
#start main movement
j = 1
while not rospy.is_shutdown():
for i in range(0,5):
joints[i].value = 0.008*j
for i in range(0,5):
vels[i].value = 0.0001*j + abs(random.random() * 0.05)
joints[2].value = -joints[2].value
vels[2].value = -vels[2].value
jp.positions = joints;
jv.velocities = vels;
#pubPos.publish(jp)
pubVel.publish(jv)
j = j+1
rate.sleep()
def joint_velocities(joint_velocities):
robot = os.getenv('ROBOT')
if robot == 'youbot-edufill':
joint_velocity_1 = joint_velocities[0]
joint_velocity_2 = joint_velocities[1]
joint_velocity_3 = joint_velocities[2]
joint_velocity_4 = joint_velocities[3]
joint_velocity_5 = joint_velocities[4]
pub = rospy.Publisher('arm_1/arm_controller/velocity_command', JointVelocities)
rospy.sleep(0.5)
try:
jv = JointVelocities()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = joint_velocity_1
jv1.unit = "s^-1 rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = joint_velocity_2
jv2.unit = "s^-1 rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = joint_velocity_3
jv3.unit = "s^-1 rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.value = joint_velocity_4
jv4.unit = "s^-1 rad"
jv5 = JointValue()
jv5.joint_uri = "arm_joint_5"
jv5.value = joint_velocity_5
jv5.unit = "s^-1 rad"
p = Poison()
#print p
jv.poisonStamp = p
jv.velocities = [jv1, jv2, jv3, jv4, jv5]
pub.publish(jv)
return 'arm moved successfully'
except Exception, e:
print e
return 'arm move failure'
def velPub():
rospy.init_node("velPub", anonymous=True)
component_name = "arm"
component_dof = 7
pub = rospy.Publisher("/" + component_name + "_controller/command_vel",
JointVelocities,
queue_size=1)
vel = JointVelocities()
for x in range(0, component_dof):
vel.velocities.append(JointValue())
r = rospy.Rate(100)
t0 = rospy.get_time()
A = 0.1 #amplitude
w = 1.0 #frequency
phi = 0.0 #phase shift
while not rospy.is_shutdown():
t = rospy.get_time()
vel.velocities[1].value = A * math.sin(w * (t - t0) + phi)
#print vel
pub.publish(vel)
r.sleep()
def get_zero_velocities(self):
jv = JointVelocities()
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_1'
jv.velocities[-1].value = 0
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_2'
jv.velocities[-1].value = 0
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_3'
jv.velocities[-1].value = 0
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_4'
jv.velocities[-1].value = 0
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_5'
jv.velocities[-1].value = 0
return jv
def get_joint_velocities(self, dx, dy): if (not self.armInitialized): dx = 0 dy = 0 mvx = 0 mvy = 0 else: mvx = self.maxVelX; mvy = self.maxVelY; if (dx>0): if self.jointConstraints[0][1]-self.jointPositions[0]<self.threshold: mvx = (self.jointConstraints[0][1]-self.jointPositions[0])*self.maxVelX; #print self.jointPositions[0], "mvx\t", mvx else: if -self.jointConstraints[0][0]+self.jointPositions[0]<self.threshold: mvx = (-self.jointConstraints[0][0]+self.jointPositions[0])*self.maxVelX; #print self.jointPositions[0], "mvx\t", mvx #print self.jointConstraints[1], "\t", self.jointPositions[1]#, "\t", (self.jointConstraints[1][1]-self.jointPositions[1]) if (dy>0): if self.jointConstraints[1][1]-self.jointPositions[1]<self.threshold: mvy = (self.jointConstraints[1][1]-self.jointPositions[1])*self.maxVelY; #print self.jointPositions[1], "mvy\t", mvy else: if -self.jointConstraints[1][0]+self.jointPositions[1]<self.threshold: mvy = (-self.jointConstraints[1][0]+self.jointPositions[1])*self.maxVelY; #print self.jointPositions[1], "mvy\t", mvy jv = JointVelocities() jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_1' jv.velocities[-1].value = dx*mvx; jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_2' jv.velocities[-1].value = dy*mvy; jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_3' jv.velocities[-1].value = 0; jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_4' jv.velocities[-1].value = 0; jv.velocities.append(JointValue()) jv.velocities[-1].unit = "s^-1 rad" jv.velocities[-1].joint_uri = 'arm_joint_5' jv.velocities[-1].value = 0; return jv
def publish_arm_joint_velocities(self, Joint_Velocities):
desiredVelocities = JointVelocities()
jointCommands = []
for i in range(5):
joint = JointValue()
joint.joint_uri = "arm_joint_" + str(i + 1)
joint.unit = "s^-1 rad"
joint.value = Joint_Velocities[i]
jointCommands.append(joint)
desiredVelocities.velocities = jointCommands
self.arm_joint_vel_pub.publish(desiredVelocities)
def go_with(self, velocities):
jv_msg = JointVelocities()
for i in range(N):
jv_msg.velocities.append(JointValue())
jv_msg.velocities[i].joint_uri = 'arm_joint_' + str(i + 1)
jv_msg.velocities[i].unit = 's^-1 rad'
jv_msg.velocities[i].value = velocities[i]
self.jv_pub.publish(jv_msg)
def main():
pubPos = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
pubVel = rospy.Publisher('/arm_1/arm_controller/velocity_command',
JointVelocities,
queue_size=10)
rospy.init_node('armcommand', anonymous=True)
rate = rospy.Rate(1) # 10hz
jp = JointPositions()
joints = []
for i in range(0, 5):
joints.append(JointValue())
joints[i].joint_uri = "arm_joint_" + str(i + 1)
joints[i].unit = "rad"
jv = JointVelocities()
vels = []
for i in range(0, 5):
vels.append(JointValue())
vels[i].joint_uri = "arm_joint_" + str(i + 1)
vels[i].unit = "s^-1 rad"
joints[0].value = 3
joints[1].value = 1
joints[2].value = -1
joints[3].value = 1.85
joints[4].value = 3
j = 1
c = 1
inverse = False
while not rospy.is_shutdown():
if not inverse:
j = j + 1
if inverse:
j = j - 1
if j > 50:
inverse = True
c = -c
joints[0].value += 0
joints[1].value += c * 0.008 * j
joints[2].value += -c * 0.008 * j
joints[3].value += 0
joints[4].value += 0
jp.positions = joints
pubPos.publish(jp)
rate.sleep()
def create_null_velocity(unit):
msg = JointVelocities()
time = rospy.Time.now()
for joint_name in joint_names:
j = JointValue()
j.timeStamp = time
j.joint_uri = joint_name
j.unit = unit
msg.velocities.append(j)
return msg
def __init__(self):
u"""Конструктор класса."""
self.base_velocity = Twist()
self.gripper_position = JointPositions()
self.arm_velocities = JointVelocities()
self.base_speed_multiplier = 0.2
self.arm_speed_multiplier = 0.5
self.base_velocity_publisher = rospy.Publisher('/cmd_vel', Twist)
self.gripper_position_publisher = rospy.Publisher(
'/arm_1/gripper_controller/position_command', JointPositions)
self.arm_velocities_publisher = rospy.Publisher(
'/arm_1/arm_controller/velocity_command', JointVelocities)
rospy.Subscriber("/joy", Joy, self.update_data_from_joy)
def createVelocity(vels):
for i in range(len(vels)):
vels[i] = float(vels[i])
vels = np.array(vels)
#clamp
if np.linalg.norm(vels, 2) > MAX_VEL:
vels /= np.linalg.norm(vels, 2)
vels *= MAX_VEL
v = JointVelocities()
ts = rospy.Time()
for i in range(5):
v.velocities.append(JointValue())
v.velocities[i].timeStamp = ts
v.velocities[i].joint_uri = arm_names[i]
v.velocities[i].unit = unit
v.velocities[i].value = float(vels[i])
return v
def main():
JP = JointVelocities()
JP.positions.append(JointValue)
JP.poisonStamp.originator = 'orig'
JP.poisonStamp.description = 'desc'
JP.poisonStamp.qos = 0.0
# JP.positions.joint_uri = 0.2
# JP.positions.unit = 'rad'
# JP.positions.value = 0.3
JP.positions[0].joint_uri = 'arm_joint_1'
JP.positions[0].unit = 'rad'
JP.positions[0].value = 0.3
planner = PlannerClass()
#planner.update([1,0,1,0,1], 0.5, -.01, False)
#action = planner.get_action()
answer = planner.update(JP, 4.5, 6.7, True)
print answer
def talker():
mouse_pos = [0,0];
#pub1 = rospy.Publisher("arm_1/arm_controller/velocity_command brics_actuator/JointVelocities", JointVelocities)
rospy.init_node('my_keyboard', anonymous=True)
r = rospy.Rate(10) # 10hz
selected_joint = 1;
pub = rospy.Publisher("arm_1/arm_controller/velocity_command", JointVelocities)
pub2 = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions)
max_v = 0.2
grasp = False;
key_input = _Getch();
while not rospy.is_shutdown():
#49 - 1 key
#print "test output"
#rospy.loginfo('str')
#while(1):
k = key_input();
# qwe 113 119 101
o = ord(k)
if o>=49 and o<=53:
selected_joint = o-48
print "joint ", o-48, " selected"
jv = JointVelocities()
jv.velocities.append(JointValue())
jv.velocities[0].joint_uri = "arm_joint_" + str(selected_joint)
jv.velocities[0].unit = "s^-1 rad"
jv.velocities[0].value = 0
if o == 61:
max_v = max_v*1.5;
print "max_v ", max_v
if o == 45:
max_v = max_v/1.5;
print "max_v ", max_v
if o == 113:
jv.velocities[0].value = -max_v
elif o == 119:
jv.velocities[0].value = 0
elif o == 101:
jv.velocities[0].value = +max_v
pub.publish(jv);
if o==32:
if grasp:
val = 0.0115
else:
val = 0.0001
jp = JointPositions()
jp.positions.append(JointValue())
jp.positions[0].unit = "m"
jp.positions[0].joint_uri = "gripper_finger_joint_l"
jp.positions[0].value = val
jp.positions.append(JointValue())
jp.positions[1].unit = "m"
jp.positions[1].joint_uri = "gripper_finger_joint_r"
jp.positions[1].value = val
pub2.publish(jp);
grasp = not grasp
# if k!='':break
print ord(k);
if ord(k)!=27:
r.sleep()
else:
exit()
import brics_actuator.msg
from brics_actuator.msg import JointPositions, JointVelocities, JointValue, Poison
import sys, select, termios, tty, signal
if __name__=="__main__":
pub = rospy.Publisher('arm_1/arm_controller/velocity_command', JointVelocities)
rospy.init_node('simple_arm_velocity')
rospy.sleep(0.5)
try:
jp = JointVelocities()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = 1.0
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = 0.5
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.5
jv3.unit = "rad"
import rospy
import copy
from cob_srvs.srv import Trigger
from brics_actuator.msg import JointVelocities
from brics_actuator.msg import JointValue
rospy.init_node("ipa_canopen_test")
rospy.wait_for_service('/tray_controller/init')
print "found init"
initService = rospy.ServiceProxy('/tray_controller/init', Trigger)
resp = initService()
velPublisher = rospy.Publisher('/tray_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv1 = JointValue()
vv1.timeStamp = rospy.Time.now()
vv1.joint_uri = "tray_1_joint"
vv2 = copy.deepcopy(vv1)
vv2.joint_uri = "tray_2_joint"
vv3 = copy.deepcopy(vv1)
vv3.joint_uri = "tray_3_joint"
v.velocities = [vv1,vv2,vv3]
v.velocities[0].value = 0.0
while not rospy.is_shutdown():
v.velocities[0].value = 0.05
v.velocities[1].value = 0.05
velPublisher.publish(v)
from brics_actuator.msg import JointPositions, JointVelocities, JointValue, Poison
import sys, select, termios, tty, signal
if __name__ == "__main__":
pub = rospy.Publisher('arm_1/arm_controller/velocity_command',
JointVelocities)
rospy.init_node('simple_arm_velocity')
rospy.sleep(0.5)
try:
jp = JointVelocities()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = 1.0
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = 0.5
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.5
jv3.unit = "rad"
gripper_pos_msg = JointPositions()
gripper_pos = JointValue()
gripper_pos.timeStamp = rospy.Time.now()
gripper_pos.joint_uri = 'left_arm_top_finger_joint'
gripper_pos.unit = 'm'
gripper_pos.value = 0.025
gripper_pos_msg.positions = [gripper_pos]
gripper_pos_pub.publish(gripper_pos_msg)
rospy.sleep(5.0)
# send joint velocities
gripper_vel_msg = JointVelocities()
gripper_vel = JointValue()
gripper_vel.timeStamp = rospy.Time.now()
gripper_vel.joint_uri = 'left_arm_top_finger_joint'
gripper_vel.unit = 'm/s'
gripper_vel_msg.velocities = [gripper_vel]
# while loop
t_start = rospy.Time.now()
A = 8e-3
f = 0.5
import rospy
from cob_srvs.srv import Trigger
from brics_actuator.msg import JointVelocities
from brics_actuator.msg import JointValue
rospy.init_node("ipa_canopen_test")
rospy.wait_for_service('/mockarm_controller/init')
initService = rospy.ServiceProxy('/mockarm_controller/init', Trigger)
resp = initService()
print resp
velPublisher = rospy.Publisher('/mockarm_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv = JointValue()
vv.timeStamp = rospy.Time.now()
vv.joint_uri = "mockarm_1_joint"
v.velocities = [vv]
while not rospy.is_shutdown():
v.velocities[0].value = 0.2
velPublisher.publish(v)
rospy.sleep(1.0)
v.velocities[0].value = 0
velPublisher.publish(v)
rospy.sleep(1.0)
import rospy
import copy
from cob_srvs.srv import Trigger
from brics_actuator.msg import JointVelocities
from brics_actuator.msg import JointValue
rospy.init_node("ipa_canopen_test")
rospy.wait_for_service('/tray_controller/init')
print "found init"
initService = rospy.ServiceProxy('/tray_controller/init', Trigger)
resp = initService()
velPublisher = rospy.Publisher('/tray_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv1 = JointValue()
vv1.timeStamp = rospy.Time.now()
vv1.joint_uri = "tray_1_joint"
vv2 = copy.deepcopy(vv1)
vv2.joint_uri = "tray_2_joint"
vv3 = copy.deepcopy(vv1)
vv3.joint_uri = "tray_3_joint"
v.velocities = [vv1, vv2, vv3]
v.velocities[0].value = 0.0
while not rospy.is_shutdown():
v.velocities[0].value = 0.05
v.velocities[1].value = 0.05
velPublisher.publish(v)
def get_joint_velocities(self, dx, dy, dz):
if (not self.armInitialized):
dx = 0
dy = 0
dz = 0
mvx = 0
mvy = 0
mvz = 0
mvexp = 0.0
else:
#print self.trackerData
dx = -self.trackerData[0]
dy = -self.trackerData[1]
if (abs(dx) < 0.13): dx = 0
if (abs(dy) < 0.13): dy = 0
mvx = self.maxVelX
mvy = self.maxVelY
if (dx > 0):
if self.jointConstraints[0][1] - self.jointPositions[
0] < self.threshold:
mvx = (self.jointConstraints[0][1] -
self.jointPositions[0]) * self.maxVelX
#print self.jointPositions[0], "mvx\t", mvx
else:
if -self.jointConstraints[0][0] + self.jointPositions[
0] < self.threshold:
mvx = (-self.jointConstraints[0][0] +
self.jointPositions[0]) * self.maxVelX
#print self.jointPositions[0], "mvx\t", mvx
#print self.jointConstraints[1], "\t", self.jointPositions[1]#, "\t", (self.jointConstraints[1][1]-self.jointPositions[1])
if (dy > 0):
if self.jointConstraints[1][1] - self.jointPositions[
1] < self.threshold:
mvy = (self.jointConstraints[1][1] -
self.jointPositions[1]) * self.maxVelY
#print self.jointPositions[1], "mvy\t", mvy
else:
if -self.jointConstraints[1][0] + self.jointPositions[
1] < self.threshold:
mvy = (-self.jointConstraints[1][0] +
self.jointPositions[1]) * self.maxVelY
#print self.jointPositions[1], "mvy\t", mvy
mvexp = 0.5
if (dz > 0):
if -0.01 - self.jointPositions[3] < 0.05:
mvexp = (-0.01 - self.jointPositions[3]) * 0.5
else:
if self.jointPositions[3] - (-2.56) < 0.05:
mvexp = (self.jointPositions[3] - (-2.56)) * 0.5
jv = JointVelocities()
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_1'
jv.velocities[-1].value = dx * mvx
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_2'
jv.velocities[-1].value = dy * mvy - mvexp * dz * 0.304
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_3'
jv.velocities[-1].value = mvexp * dz
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_4'
jv.velocities[-1].value = -mvexp * dz * 0.671
jv.velocities.append(JointValue())
jv.velocities[-1].unit = "s^-1 rad"
jv.velocities[-1].joint_uri = 'arm_joint_5'
jv.velocities[-1].value = 0
return jv
