def to_joint_positions(gripper_positions):
robot = os.getenv('ROBOT')
if robot == 'youbot-edufill':
left_gripper_val = gripper_positions[0]
right_gripper_val = gripper_positions[1]
pub = rospy.Publisher("/arm_1/gripper_controller/position_command",
JointPositions)
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left_gripper_val
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right_gripper_val
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
pub.publish(jp)
rospy.sleep(1.0)
rospy.loginfo('Gripper control command published successfully')
except Exception, e:
rospy.loginfo('%s', e)
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 jsPoscontrol(self, qd):
#Initialize Position Command Msgs
qposMsg = JointPositions()
qposMsg.positions = [
JointValue(),
JointValue(),
JointValue(),
JointValue(),
JointValue()
]
#Fill Position Msg
qposMsg.positions[0].joint_uri = 'arm_joint_1'
qposMsg.positions[0].unit = 'rad'
qposMsg.positions[0].value = qd[0] + self.q_offset[0]
qposMsg.positions[1].joint_uri = 'arm_joint_2'
qposMsg.positions[1].unit = 'rad'
qposMsg.positions[1].value = qd[1] + self.q_offset[1]
qposMsg.positions[2].joint_uri = 'arm_joint_3'
qposMsg.positions[2].unit = 'rad'
qposMsg.positions[2].value = qd[2] + self.q_offset[2]
qposMsg.positions[3].joint_uri = 'arm_joint_4'
qposMsg.positions[3].unit = 'rad'
qposMsg.positions[3].value = qd[3] + self.q_offset[3]
qposMsg.positions[4].joint_uri = 'arm_joint_5'
qposMsg.positions[4].unit = 'rad'
qposMsg.positions[4].value = qd[4] + self.q_offset[4]
#Publish Position Msg
self.pub_qpos.publish(qposMsg)
def make_brics_msg_gripper(cls, arm_num, opening_m):
# Turn a desired gripper opening into a brics-friendly message
left = opening_m/2.0
right = opening_m/2.0
# create joint positions message
jp = JointPositions()
# create joint values message for both left and right fingers
jvl = JointValue()
jvr = JointValue()
# Fill in the gripper positions desired
# This is open position (max opening 0.0115 m)
if arm_num == 1:
jvl.joint_uri = 'gripper_finger_joint_l'
jvr.joint_uri = 'gripper_finger_joint_r'
else:
jvl.joint_uri = 'gripper_' + str(arm_num) + '_finger_joint_l'
jvr.joint_uri = 'gripper_' + str(arm_num) + '_finger_joint_r'
jvl.unit = 'm'
jvl.value = left
jvr.unit = 'm'
jvr.value = right
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jvl))
jp.positions.append(copy.deepcopy(jvr))
return jp
def make_grip_msg(opening_mm, joint_uri):
left = opening_mm / 2000.
right = opening_mm / 2000.
# create joint positions message
jp = JointPositions()
# create joint values message for both left and right fingers
jvl = JointValue()
jvr = JointValue()
# Fill in the gripper positions desired
# This is open position (max opening 0.0115 m)
jvl.joint_uri = joint_uri[5]
jvl.unit = 'm'
jvl.value = left
jvr.joint_uri = joint_uri[6]
jvr.unit = 'm'
jvr.value = right
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jvl))
jp.positions.append(copy.deepcopy(jvr))
return jp
def gripperCommand(data):
'''moves the gripper to a predefined configuration.'''
rospy.loginfo(rospy.get_caller_id() + "gripper data: %s", data.data)
#creating a JointValue() List/Array
jvaluegrippers = [0 for i in range(2)]
#creating a JointValue() Object for gripper left
jvalue6 = JointValue()
jvalue6.joint_uri = 'gripper_finger_joint_l'
jvalue6.unit = 'm'
jvalue6.value = data.data[0]
jvaluegrippers[0] = jvalue6
#creating a JointValue() Object for gripper right
jvalue7 = JointValue()
jvalue7.joint_uri = 'gripper_finger_joint_r'
jvalue7.unit = 'm'
jvalue7.value = data.data[1]
jvaluegrippers[1] = jvalue7
#setting JointPosition msg properties
rate = rospy.Rate(20)
msg2 = JointPositions()
msg2.positions = jvaluegrippers
pubGripper.publish(msg2)
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 make_brics_msg_gripper(opening_m):
# Turn a desired gripper opening into a brics-friendly message
left = opening_m/2.
right = opening_m/2.
# create joint positions message
jp = JointPositions()
# create joint values message for both left and right fingers
jvl = JointValue()
jvr = JointValue()
# Fill in the gripper positions desired
# This is open position (max opening 0.0115 m)
jvl.joint_uri = 'gripper_finger_joint_l'
jvl.unit = 'm'
jvl.value = left
jvr.joint_uri = 'gripper_finger_joint_r'
jvr.unit = 'm'
jvr.value = right
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jvl))
jp.positions.append(copy.deepcopy(jvr))
return jp
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 Set(self,value):
grp_cmd = JointPositions()
j_cmd = JointValue()
j_cmd.joint_uri = self.joint_name
j_cmd.unit = 'm'
j_cmd.value = value
grp_cmd.positions.append(j_cmd)
print "Sending gripper command: " + str(j_cmd.value)
self._GripperCmdPublisher.publish(grp_cmd)
def create_joint_value_msg_list(self, joint_values, unit):
joint_value_msg_list = []
for i, joint_value in enumerate(joint_values):
joint_value_msg = JointValue()
joint_value_msg.joint_uri = 'arm_joint_' + str(i + 1)
joint_value_msg.unit = unit
joint_value_msg.value = joint_value
joint_value_msg_list.append(joint_value_msg)
return joint_value_msg_list
def make_brics_msg_arm(positions):
# create joint positions message
jp = JointPositions()
for ii in range(5):
jv = JointValue()
jv.joint_uri = 'arm_joint_' + str(ii+1)
jv.unit='rad'
jv.value = positions[ii]
jp.positions.append(copy.deepcopy(jv))
return jp
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 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 createGripperPositionCommand(newPos, t):
msg = JointPositions()
joint = JointValue(timeStamp=t,
value=newPos,
unit="m",
joint_uri="gripper_finger_joint_l")
msg.positions.append(joint)
joint.joint_uri = "gripper_finger_joint_r"
msg.positions.append(joint)
return msg
def _createJointPositions(self, joint_config):
jp = JointPositions()
for i in range(5):
jv = JointValue()
jv.joint_uri = self.joint_names[i]
jv.value = joint_config[i]
jv.unit = "rad"
jp.positions.append(jv)
return jp
def to_joint_positions(gripper_positions):
robot = os.getenv("ROBOT")
if robot == "youbot-edufill":
left_gripper_val = gripper_positions[0]
right_gripper_val = gripper_positions[1]
pub = rospy.Publisher("/arm_1/gripper_controller/position_command", JointPositions)
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left_gripper_val
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right_gripper_val
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] # list
pub.publish(jp)
rospy.sleep(1.0)
rospy.loginfo("Gripper control command published successfully")
except Exception, e:
rospy.loginfo("%s", e)
def make_grip_msg(opening_mm, joint_uri):
left = opening_mm/2000.
right = opening_mm/2000.
# create joint positions message
jp = JointPositions()
# create joint values message for both left and right fingers
jvl = JointValue()
jvr = JointValue()
# Fill in the gripper positions desired
# This is open position (max opening 0.0115 m)
jvl.joint_uri = joint_uri[5]
jvl.unit = 'm'
jvl.value = left
jvr.joint_uri = joint_uri[6]
jvr.unit = 'm'
jvr.value = right
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jvl))
jp.positions.append(copy.deepcopy(jvr))
return jp
def create_gripper_msg(left_gripper, right_gripper):
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left_gripper
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right_gripper
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2]
return jp
def execute(self, goal):
rospy.logdebug("Executing command goal: " + str(goal))
jnt_pos = JointPositions()
for k in range(len(self.joint_names)):
value = JointValue()
value.joint_uri = self.joint_names[k]
value.unit = "m"
value.value = goal.command.position
jnt_pos.positions.append(value)
self.pub_joint_positions.publish(jnt_pos)
rospy.sleep(2.0)
self.server.set_succeeded()
def arm_up_recover(self):
arm_up = create_arm_up()
arm_up[0] = self.configuration[0]
arm_up[4] = self.configuration[4]
joint_positions = JointPositions()
# transform from ROS to BRICS message
for j in range(len(joint_names)):
joint_value = JointValue()
joint_value.joint_uri = joint_names[j]
joint_value.value = limit_joint(joint_names[j], arm_up[j])
joint_value.unit = self.unit
joint_positions.positions.append(joint_value)
self.position_pub.publish(joint_positions)
rospy.sleep(1)
def arm_up_recover(self):
arm_up = create_arm_up()
arm_up[0] = self.configuration[0]
arm_up[4] = self.configuration[4]
joint_positions = JointPositions()
# transform from ROS to BRICS message
for j in range(len(joint_names)):
joint_value = JointValue()
joint_value.joint_uri = joint_names[j]
joint_value.value = limit_joint(joint_names[j], arm_up[j])
joint_value.unit = self.unit
joint_positions.positions.append(joint_value)
self.position_pub.publish(joint_positions)
rospy.sleep(1)
def main():
pubPos = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
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"
while not rospy.is_shutdown():
joints[0].value = 3
joints[1].value = 1
joints[2].value = -1
joints[3].value = 1.85
joints[4].value = 3
jp.positions = joints
pubPos.publish(jp)
rate.sleep()
def get_joint_pos_msg_from_joint_angles(joint_angles):
"""
Converts a list of joint angles to a JointPositions message
:joint_angles: list(float)
:returns: brics_actuator.JointPositions
"""
joint_position_msg = JointPositions()
for i, joint_angle in enumerate(joint_angles):
joint_value_msg = JointValue()
joint_value_msg.joint_uri = 'arm_joint_' + str(i + 1)
joint_value_msg.unit = 'rad'
joint_value_msg.value = joint_angle
joint_position_msg.positions.append(joint_value_msg)
return joint_position_msg
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 poseCallback(simple_msg):
hard_msg = JointPositions()
for i in range(5):
hard_msg.positions.append(JointValue())
hard_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
hard_msg.positions[i].unit = 'rad'
hard_msg.positions[i].value = simple_msg.data[i]
pose_pub.publish(hard_msg)
def publish_arm_joint_positions(self, joint_positions):
desiredPositions = JointPositions()
jointCommands = []
for i in range(5):
joint = JointValue()
joint.joint_uri = "arm_joint_" + str(i+1)
joint.unit = "rad"
joint.value = joint_positions[i]
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.arm_joint_pub.publish(desiredPositions)
def go_with2(self, torques):
jt_msg = JointTorques()
for i in range(N):
jt_msg.torques.append(JointValue())
jt_msg.torques[i].joint_uri = 'arm_joint_' + str(i + 1)
jt_msg.torques[i].unit = 'm^2 kg s^-2 rad^-1'
jt_msg.torques[i].value = torques[i]
self.jt_pub.publish(jt_msg)
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_to(self, position):
jp_msg = JointPositions()
for i in range(N):
jp_msg.positions.append(JointValue())
jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
jp_msg.positions[i].unit = 'rad'
jp_msg.positions[i].value = position[i]
self.jp_pub.publish(jp_msg)
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 publish_arm_joint_positions(self, joint_positions):
desiredPositions = JointPositions()
jointCommands = []
for i in range(5):
joint = JointValue()
joint.joint_uri = "arm_joint_" + str(i + 1)
joint.unit = "rad"
joint.value = joint_positions[i]
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.arm_joint_pub.publish(desiredPositions)
def createArmPositionCommand(newArmPosition, t):
msg = JointPositions()
for i, val in enumerate(newArmPosition, 1):
msg.positions.append(
JointValue(timeStamp=t,
value=val * pi / 180,
unit="rad",
joint_uri="arm_joint_%s" % (i, )))
return msg
def publish_gripper_width(self, width):
desiredPositions = JointPositions()
jointCommands = []
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_l"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_r"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.gripper_pub.publish(desiredPositions)
def set_joints_angles(self, *args): # TODO: remove *args
u"""Set arm joints to defined angles in radians.
Arguments:
*args -- joints angles (j1, j2, j3, j4, j5)
Устанавливает углы поворота степеней подвижности в радианах.
Аргументы:
*args -- уголы соотвествующих степеней (j1, j2, j3, j4, j5)
"""
assert len(args) <= 5
self.joints_positions.positions = []
for i in range(5):
tmp = JointValue()
tmp.timeStamp = rospy.Time.now()
tmp.joint_uri = 'arm_joint_{}'.format(i + 1)
tmp.unit = 'rad'
tmp.value = args[i]
self.joints_positions.positions.append(tmp)
self.joints_positions_publisher.publish(self.joints_positions)
def moveGripper(self, gPublisher, floatVal):
jp = JointPositions()
myPoison = Poison()
myPoison.originator = 'yb_grip'
myPoison.description = 'whoknows'
myPoison.qos = 0.0
jp.poisonStamp = myPoison
nowTime = rospy.Time.now()
jvl = JointValue()
jvl.timeStamp = nowTime
jvl.joint_uri = 'gripper_finger_joint_l'
jvl.unit = 'm'
jvl.value = floatVal
jp.positions.append(jvl)
jvr = JointValue()
jvr.timeStamp = nowTime
jvr.joint_uri = 'gripper_finger_joint_r'
jvr.unit = 'm'
jvr.value = floatVal
jp.positions.append(jvr)
gPublisher.publish(jp)
def set_joints_velocities(self, *args): # TODO: remove *args
u"""Set velocity for each joint.
Arguments:
*args -- velocity for each joint (j1, j2, j3, j4, j5)
Устанавливает скорость каждой степени подвижности в радианах/с.
Аргументы:
*args -- скорости соотвествующих степеней (j1, j2, j3, j4, j5)
"""
assert len(args) == 5
self.joints_velocities.velocities = []
for index, value in enumerate(args):
tmp = JointValue()
tmp.timeStamp = rospy.Time.now()
tmp.joint_uri = 'arm_joint_{}'.format(index + 1)
tmp.unit = 's^-1 rad'
tmp.value = value
self.joints_velocities.velocities.append(tmp)
self.joints_velocities_publisher.publish(self.joints_velocities)
def goToPose(self, q):
jp_msg = JointPositions()
# for i in range(1):
# jp_msg.positions.append(JointValue())
# jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i+1)
# jp_msg.positions[i].unit = 'rad'
jp_msg.positions.append(JointValue())
jp_msg.positions[0].joint_uri = 'arm_joint_5'
jp_msg.positions[0].unit = 'rad'
jp_msg.positions[0].value = q
self.jp_pub.publish(jp_msg)
def main(self):
# rospy.init_node('putitback')
arm_pub = rospy.Publisher('/arm_1/arm_controller/position_command',
JointPositions,
queue_size=10)
# arm_ik_control.go_to_xyz(0.1, -0.2, 0.3, arm_pub)
initial_position.main()
gripper_open.main()
msg = JointPositions()
rospy.sleep(5)
print "moving joint 1" #move joint 3 and joint 2
msg.positions = [JointValue()]
msg.positions[0].timeStamp = rospy.Time.now()
msg.positions[0].unit = "rad"
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].value = 0.04
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_5"
msg.positions[0].value = 2.8
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_1"
msg.positions[0].value = 0.01005
arm_pub.publish(msg)
rospy.sleep(3)
print "moving joint 2"
msg.positions[0].joint_uri = "arm_joint_4"
msg.positions[0].value = 1.7
arm_pub.publish(msg)
rospy.sleep(2)
msg.positions[0].joint_uri = "arm_joint_3"
msg.positions[0].value = -0.6
arm_pub.publish(msg)
rospy.sleep(1)
msg.positions[0].joint_uri = "arm_joint_2"
msg.positions[0].value = 1.4
arm_pub.publish(msg)
gripper_close.main()
rospy.sleep(2)
initial_position.main()
arm_ik_control.go_to_xyz_rev(-0.069, -0.447, 0.12, arm_pub)
def create_gripper_msg (left_gripper, right_gripper): jp = JointPositions() jv1 = JointValue() jv1.joint_uri = "gripper_finger_joint_l" jv1.value = left_gripper jv1.unit = "m" jv2 = JointValue() jv2.joint_uri = "gripper_finger_joint_r" jv2.value = right_gripper jv2.unit = "m" p = Poison() jp.poisonStamp = p jp.positions = [jv1, jv2] return jp
def make_movements_callback(req):
global file_with_data
jp_pub = rospy.Publisher("arm_1/arm_controller/position_command",
JointPositions,
queue_size=10,
latch=True)
jp_msg = JointPositions()
for i in range(5):
jp_msg.positions.append(JointValue())
jp_msg.positions[i].joint_uri = 'arm_joint_' + str(i + 1)
jp_msg.positions[i].unit = 'rad'
file_with_poses = open(fwp_name, "r")
string_of_poses = file_with_poses.readlines()
file_with_poses.close()
js_sub = rospy.Subscriber("joint_states", JointState, js_callback)
r = rospy.Rate(1.0)
for pose in string_of_poses:
angles = [float(angle) for angle in pose.split()]
for i in range(5):
jp_msg.positions[i].value = angles[i]
jp_pub.publish(jp_msg)
achieved = False
while not achieved:
achieved = True
r.sleep()
lock.acquire()
for i in range(5):
if abs(angles[i] - cur_angles[i]) > 0.2:
achieved = False
break
lock.release()
js_sub.unregister()
# for returning to startup (embryo) position
for i in range(5):
jp_msg.positions[i].value = 0.025
jp_msg.positions[2].value = -0.02
jp_msg.positions[4].value = 0.12
jp_pub.publish(jp_msg)
lock.acquire()
file_with_data.close()
lock.release()
return EmptyResponse()
def _createGripperJointPositions(self, left, right):
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = left
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = right
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
return jp
def publish_gripper_width(self, width):
desiredPositions = JointPositions()
jointCommands = []
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_l"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_r"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.gripper_pub.publish(desiredPositions)
joint_position_string_pub = rospy.Publisher("/robust_position_controller/event_in", String, queue_size=100)
joint_position_msg = JointPositions()
string_msg= String()
string_msg.data = "e_start"
joint_names = ["arm_joint_1" , "arm_joint_5"]
joint_value_=[4.0, 4.0]
for i in range (0,len(joint_names)):
joint_value_msg = JointValue()
joint_value_msg.joint_uri = joint_names[i]
joint_value_msg.value = joint_value_[i]
joint_value_msg.unit="rad"
joint_position_msg.positions.append(joint_value_msg)
loop_rate = rospy.Rate(10)
while not rospy.is_shutdown():
joint_position_pub.publish(joint_position_msg)
rospy.sleep(0.5)
joint_position_string_pub.publish(string_msg)
loop_rate.sleep()
import sys, select, termios, tty, signal
if __name__=="__main__":
pub = rospy.Publisher('/arm_1/gripper_controller/position_command', JointPositions)
rospy.init_node('simple_gripper_joint_position')
rospy.sleep(0.5)
try:
jp = JointPositions()
# if value == 0: gripper is closed
# if value == 0.0115: griper is open
jv1 = JointValue()
jv1.joint_uri = "gripper_finger_joint_l"
jv1.value = 0.01
jv1.unit = "m"
jv2 = JointValue()
jv2.joint_uri = "gripper_finger_joint_r"
jv2.value = 0.01
jv2.unit = "m"
p = Poison()
jp.poisonStamp = p
jp.positions = [jv1, jv2] #list
def main(stdscr, persist):
pub = rospy.Publisher('out',JointPositions, queue_size=1)
rospy.init_node('bras_teleop', anonymous=True)
theta1, theta2, theta3, theta4, theta5 = 0.111,0.11,-0.11,0.11,0.111
jointvalues = [theta1,theta2,theta3,theta4,theta5]
keycode = -1
rate = rospy.Rate(100)
stdscr.addstr("Command\n")
stdscr.addstr("- a/q : theta1 ++/--\n")
stdscr.addstr("- z/s : theta2 ++/--\n")
stdscr.addstr("- e/d : theta3 ++/--\n")
stdscr.addstr("- r/f : theta4 ++/--\n")
stdscr.addstr("- t/g : theta5 ++/--\n")
stdscr.addstr("- ESC : exit\n")
# We will wait for 100 ms for a key to be pressed
if(persist): stdscr.timeout(100)
while (not rospy.is_shutdown()) and (keycode != 27): # 27 is escape
keycode = stdscr.getch() # read pressed key
if keycode == -1 : pass # No key has been pressed
elif keycode == ord('a') :
if(jointvalues[0]+0.05> 0.0100692 and jointvalues[0]+0.05<5.84014 ):jointvalues[0]+=0.05
elif keycode == ord('q') :
if(jointvalues[0]-0.05>0.0100692 and jointvalues[0]-0.05< 5.84014 ):jointvalues[0]-=0.05
elif keycode == ord('z') :
if(jointvalues[1]+0.05> 0.0100692 and jointvalues[1]+0.05< 2.61799 ):jointvalues[1]+=0.05
elif keycode == ord('s') :
if(jointvalues[1]-0.05>0.0100692 and jointvalues[1]-0.05< 2.61799 ):jointvalues[1]-=0.05
elif keycode == ord('e') :
if(jointvalues[2]+0.05>-5.02655 and jointvalues[2]+0.05<-0.015708 ):jointvalues[2]+=0.05
elif keycode == ord('d') :
if(jointvalues[2]-0.05>-5.02655 and jointvalues[2]-0.05<-0.015708 ):jointvalues[2]-=0.05
elif keycode == ord('r') :
if(jointvalues[3]+0.05>0.0221239 and jointvalues[3]+0.05<3.4292 ):jointvalues[3]+=0.05
elif keycode == ord('f') :
if(jointvalues[3]-0.05>0.0221239 and jointvalues[3]-0.05<3.4292 ):jointvalues[3]-=0.05
elif keycode == ord('t') : jointvalues[4]+=0.05
elif keycode == ord('g') : jointvalues[4]-=0.05
jp = JointPositions()
jv1 = JointValue()
jv2 = JointValue()
jv3 = JointValue()
jv4 = JointValue()
jv5 = JointValue()
jv1.joint_uri ="arm_joint_1"
jv1.unit = "rad"
jv1.value =jointvalues[0]
jv2.joint_uri ="arm_joint_2"
jv2.unit = "rad"
jv2.value = jointvalues[1]
jv3.joint_uri = "arm_joint_3"
jv3.unit = "rad"
jv3.value = jointvalues[2]
jv4.joint_uri = "arm_joint_4"
jv4.unit = "rad"
jv4.value = jointvalues[3]
jv5.joint_uri = "arm_joint_5"
jv5.unit = "rad"
jv5.value = jointvalues[4]
jp.positions.append(copy.deepcopy(jv1))
jp.positions.append(copy.deepcopy(jv2))
jp.positions.append(copy.deepcopy(jv3))
jp.positions.append(copy.deepcopy(jv4))
jp.positions.append(copy.deepcopy(jv5))
pub.publish(jp)
rate.sleep()
from brics_actuator.msg import JointPositions, JointValue, Poison
import sys, select, termios, tty, signal
if __name__=="__main__":
pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
rospy.init_node('simple_arm_gripper_position')
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = 0.011
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = 0.011
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = -0.016
jv3.unit = "rad"
jv4 = JointValue()
def make_arm_msg(arm_js, joint_uri):
A1 = arm_js[0]
A2 = arm_js[1]
A3 = arm_js[2]
A4 = arm_js[3]
A5 = arm_js[4]
# create joint positions message
jp = JointPositions()
# create joint values message for all the joints
jv1 = JointValue()
jv2 = JointValue()
jv3 = JointValue()
jv4 = JointValue()
jv5 = JointValue()
# Fill in the arm positions.
jv1.joint_uri = joint_uri[0]
jv1.unit = 'rad'
jv1.value = A1
jv2.joint_uri = joint_uri[1]
jv2.unit = 'rad'
jv2.value = A2
jv3.joint_uri = joint_uri[2]
jv3.unit = 'rad'
jv3.value = A3
jv4.joint_uri = joint_uri[3]
jv4.unit = 'rad'
jv4.value = A4
jv5.joint_uri = joint_uri[4]
jv5.unit = 'rad'
jv5.value = A5
# Append those onto JointPositions
jp.positions.append(copy.deepcopy(jv1))
jp.positions.append(copy.deepcopy(jv2))
jp.positions.append(copy.deepcopy(jv3))
jp.positions.append(copy.deepcopy(jv4))
jp.positions.append(copy.deepcopy(jv5))
return jp
if __name__=="__main__":
pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
rospy.init_node('simple_arm_gripper_position')
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.unit = "rad"
jv4 = JointValue()
jv4.joint_uri = "arm_joint_4"
jv4.unit = "rad"
pose.pose.position.z = z
quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
pose.pose.orientation.x = quat[0]
pose.pose.orientation.y = quat[1]
pose.pose.orientation.z = quat[2]
pose.pose.orientation.w = quat[3]
pose.header.frame_id = "/arm_link_0"
pose.header.stamp = rospy.Time.now()
# (conf, success) = iks.call_ik_solver(pose)
(conf, success) = iks.call_constraint_aware_ik_solver(pose)
if (success):
# publish solution directly as joint positions
print(conf)
jp = JointPositions()
for i in range(5):
jv = JointValue()
jv.joint_uri = iks.joint_names[i]
jv.value = conf[i]
jv.unit = "rad"
jp.positions.append(jv)
rospy.sleep(0.5)
print "publishing cmd"
armpub.publish(jp)
else:
print("IK solver didn't find a solution")
def execute(self, component_name, target="", blocking=True):
ah = ActionHandle("move_arm_joint_direct", component_name, target, blocking)
rospy.loginfo("Move <<%s>> DIRECT to <<%s>>", component_name, target)
# check pose
if not type(target) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
print "parameter is:", target
ah.set_failed(3)
return ah
else:
#print i,"type1 = ", type(i)
DOF = 5
if not len(target) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(target))
print "parameter is:", target
ah.set_failed(3)
return ah
else:
for i in target:
#print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
#print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", target
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
pose_goal = MoveToJointConfigurationGoal()
for i in range(DOF):
jv = JointValue()
jv.joint_uri = self.arm1_joint_names[i]
jv.value = target[i]
jv.unit = "rad"
pose_goal.goal.positions.append(jv)
rospy.logdebug("calling %s action server", self.action_server_name)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start", self.action_server_name)
if not self.client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", self.action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", self.action_server_name)
#print client_goal
self.client.send_goal(pose_goal)
ah.set_client(self.client)
ah.wait_inside()
return ah
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 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 processJoints(self, joint_states):
armStatesReceived = joint_states.name[0] == 'arm_joint_1'
if armStatesReceived and self.joyMsg is not None:
self.crntJointStates = copy.deepcopy(joint_states)
for index in range(0, len(joint_states.position)-2):
pos = JointValue()
pos.joint_uri = "arm_joint_" + str(index + 1)
pos.unit = "rad"
pos.value = joint_states.position[index]
self.crntJointPositions.positions.append(pos)
pos = JointValue()
pos.joint_uri = "gripper_finger_joint_l"
pos.unit = "m"
pos.value = joint_states.position[5]
self.gripperPositions.positions.append(pos)
pos = JointValue()
pos.joint_uri = "gripper_finger_joint_r"
pos.unit = "m"
pos.value = joint_states.position[6]
self.gripperPositions.positions.append(pos)
# desiredPos = copy.deepcopy(self.crntJointPositions)
desiredGripPos = copy.deepcopy(self.gripperPositions)
self.joyMsg.axes = numpy.array(self.joyMsg.axes)
left_axis = self.joyMsg.axes[[0, 1]]
right_axis = self.joyMsg.axes[[2, 5]]
l2_axis = self.joyMsg.axes[[3]] * -1
r2_axis = self.joyMsg.axes[[4]] * -1
square, x, circle, triangle, l1, r1, l2, r2, share, options, l3, r3, ps_button, touch_button = [button == 1 for button in self.joyMsg.buttons]
left_dpad = self.joyMsg.axes[6] == 1
right_dpad = self.joyMsg.axes[6] == -1
up_dpad = self.joyMsg.axes[7] == 1
down_dpad = self.joyMsg.axes[7] == -1
if not self.previous_vel_change:
if square:
self.maxVel += 0.25
if circle:
self.maxVel -= 0.25
if self.maxVel < 0.25:
self.maxVel = 0.25
self.previous_vel_change = square or circle
# object that will carry all desired velocities to each joint
desiredVel = JointVelocities()
for index in range(0, len(joint_states.position)-2):
vel = JointValue()
vel.joint_uri = "arm_joint_" + str(index + 1)
vel.unit = "s^-1 rad" # radians per second
desiredVel.velocities.append(vel)
desiredVel.velocities[0].value -= left_axis[0] * self.maxVel
desiredVel.velocities[1].value -= left_axis[1] * self.maxVel
if not r1:
desiredVel.velocities[2].value -= right_axis[1] * self.maxVel
else:
desiredVel.velocities[3].value -= right_axis[1] * self.maxVel
desiredVel.velocities[4].value -= right_axis[0] * self.maxVel * 2
desiredGripPos.positions[0].value -= 0.0115 * r2_axis[0]
desiredGripPos.positions[1].value = desiredGripPos.positions[0].value
joint_message = ''
vel_message = ''
grip_message = ''
for jointNum in range(5):
joint_message += ('joint ' + str(jointNum) + ': ' + str(self.crntJointPositions.positions[jointNum].value) + ' ')
vel_message += ('veloc ' + str(jointNum) + ': ' + str(desiredVel.velocities[jointNum].value) + ' ')
for gripNum in [0, 1]:
desiredGripPos.positions[gripNum].value = self.checkValues(desiredGripPos.positions[gripNum].value, gripNum + 5)
grip_message += ('grip ' + str(gripNum) + ': ' + str(desiredGripPos.positions[gripNum].value) + ' ')
self.gripperPositions = desiredGripPos
print(joint_message)
print(vel_message)
print(grip_message)
self.gripPub.publish(desiredGripPos)
self.velPub.publish(desiredVel)
from brics_actuator.msg import JointPositions, JointValue, Poison
import sys, select, termios, tty, signal
if __name__=="__main__":
pub = rospy.Publisher('arm_1/arm_controller/position_command', JointPositions)
rospy.init_node('simple_arm_gripper_position')
rospy.sleep(0.5)
try:
jp = JointPositions()
jv1 = JointValue()
jv1.joint_uri = "arm_joint_1"
jv1.value = 0.0
jv1.unit = "rad"
jv2 = JointValue()
jv2.joint_uri = "arm_joint_2"
jv2.value = 0.0
jv2.unit = "rad"
jv3 = JointValue()
jv3.joint_uri = "arm_joint_3"
jv3.value = 0.0
jv3.unit = "rad"
jv4 = JointValue()
def execute(self, target="", blocking=True):
component_name = "gripper"
ah = ActionHandle("move_gripper", component_name, target, blocking)
rospy.loginfo("Move <<%s>> to <<%s>>", component_name, target)
# get pose from parameter server
if type(target) is str:
if not rospy.has_param(self.action_server_name_prefix + "/" + component_name + "/" + target):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...", self.action_server_name_prefix + "/" + component_name + "/" + target)
ah.set_failed(2)
return ah
param = rospy.get_param(self.action_server_name_prefix + "/" + component_name + "/" + target)
else:
param = target
# check pose
"""
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for %s: not a list, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
print i,"type1 = ", type(i)
DOF = 2
if not len(param) == DOF: # check dimension
rospy.logerr("no valid parameter for %s: dimension should be %d and is %d, aborting...", component_name, DOF, len(param))
print "parameter is:", param
ah.set_failed(3)
return ah
else:
for i in param:
print i,"type2 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
print type(i)
rospy.logerr("no valid parameter for %s: not a list of float or int, aborting...", component_name)
print "parameter is:", param
ah.set_failed(3)
return ah
else:
rospy.logdebug("accepted parameter %f for %s", i, component_name)
"""
pose_goal = MoveToJointConfigurationGoal()
DOF = 2
for i in range(DOF):
jv = JointValue()
jv.joint_uri = self.gripper1_joint_names[i]
jv.value = param[i]
jv.unit = "m"
pose_goal.goal.positions.append(jv)
action_server_name = "/arm_1/gripper_controller/MoveToJointConfigurationDirect"
rospy.logdebug("calling %s action server", action_server_name)
client = actionlib.SimpleActionClient(action_server_name, MoveToJointConfigurationAction)
# trying to connect to server
rospy.logdebug("waiting for %s action server to start", action_server_name)
if not client.wait_for_server(rospy.Duration(5)):
# error: server did not respond
rospy.logerr("%s action server not ready within timeout, aborting...", action_server_name)
ah.set_failed(4)
return ah
else:
rospy.logdebug("%s action server ready", action_server_name)
print pose_goal
client.send_goal(pose_goal)
ah.set_client(client)
ah.wait_inside()
# it should block but it does not, so we wait!
rospy.sleep(3.0)
return ah
#!/usr/bin/env python
import roslib
roslib.load_manifest('youbot_pick_and_place')
import rospy
import sys
import select, termios, tty, signal
import moveit_commander, moveit_msgs.msg
from brics_actuator.msg import JointPositions, JointValue
import geometry_msgs.msg
gripper_l = JointValue()
gripper_l.joint_uri = 'gripper_finger_joint_l'
gripper_l.unit = 'm'
gripper_r = JointValue()
gripper_r.joint_uri = 'gripper_finger_joint_r'
gripper_r.unit = 'm'
joint_1 = JointValue()
joint_1.joint_uri = 'arm_joint_1'
joint_1.unit = 'rad'
joint_2 = JointValue()
joint_2.joint_uri = 'arm_joint_2'
joint_2.unit = 'rad'
joint_3 = JointValue()
joint_3.joint_uri = 'arm_joint_3'
joint_3.unit = 'rad'
joint_4 = JointValue()
joint_4.joint_uri = 'arm_joint_4'
joint_4.unit = 'rad'
joint_5 = JointValue()
joint_5.joint_uri = 'arm_joint_5'
joint_5.unit = 'rad'
def set_gripper_state(self, open_gripper=True):
"""Open/close gripper.
Arguments:
open_gripper (bool): if True - opens gripper, if False - otherwise
"""
self.gripper_position.positions = []
if open_gripper:
# Open gripper
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0.011499
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0.011499
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
else:
# Close gripper
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
self.gripper_position_publisher.publish(self.gripper_position)
