def set_jep(self, arm, q, duration=None):
if arm == 'right_arm':
pub = self.r_jep_cmd_pub
elif arm == 'left_arm':
pub = self.l_jep_cmd_pub
else:
raise RuntimeError('Undefined arm: %s'%(arm))
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
pub.publish(FloatArray(h, q))
def get_new_path(self):
from nav_msgs.msg import Path
from roslib.msg import Header
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from geometry_msgs.msg import Point
from geometry_msgs.msg import Quaternion
return Path(None, [PoseStamped(Header(0,rospy.Time(0,0),'foo'), Pose(Point(1.23, 4.56, 7.89), Quaternion(1,0,0,1))),
PoseStamped(Header(0,rospy.Time(0,1000),'foo'), Pose(Point(1.25, 4.58, 7.91), Quaternion(0,1,0,1))),
PoseStamped(Header(0,rospy.Time(0,2000),'foo'), Pose(Point(1.27, 4.60, 7.93), Quaternion(0,0,1,1)))])
def set_impedance_scale(self, arm, s):
if arm == 'right_arm':
pub = self.r_alph_cmd_pub
elif arm == 'left_arm':
pub = self.l_alph_cmd_pub
else:
raise RuntimeError('Undefined arm: %s'%(arm))
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
pub.publish(FloatArray(h, s))
def set_jep(self, arm, q, duration=None):
if arm == 'right_arm':
pub = self.r_jep_cmd_pub
elif arm == 'left_arm':
pub = self.l_jep_cmd_pub
else:
raise RuntimeError('Undefined arm: %s' % (arm))
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
pub.publish(FloatArray(h, q))
def set_impedance_scale(self, arm, s):
if arm == 'right_arm':
pub = self.r_alph_cmd_pub
elif arm == 'left_arm':
pub = self.l_alph_cmd_pub
else:
raise RuntimeError('Undefined arm: %s' % (arm))
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
pub.publish(FloatArray(h, s))
def get_new_occupancy_grid(self): from nav_msgs.msg import OccupancyGrid from nav_msgs.msg import MapMetaData from roslib.msg import Header from geometry_msgs.msg import Pose from geometry_msgs.msg import Point from geometry_msgs.msg import Quaternion import random r = random.Random(1234) return OccupancyGrid(Header(0, rospy.Time(0,0), "/map"), MapMetaData(rospy.Time(123,456), 0.1, 100, 100, Pose(Point(1.23, 4.56, 7.89), Quaternion(0,0,0,1))), [r.randint(-1,100) for x in xrange(0,1000)])
def init_goal(self, goal, transition_cb=None, feedback_cb=None):
action_goal = self.ActionGoal(header=Header(),
goal_id=self._generate_id(),
goal=goal)
csm = CommStateMachine(action_goal, transition_cb, feedback_cb,
self.send_goal_fn, self.cancel_fn)
self.list_mutex.acquire()
try:
self.statuses.append(weakref.ref(csm))
finally:
self.list_mutex.release()
self.send_goal_fn(action_goal)
return GoalHandle(csm)
def get_new_joy(self):
from sensor_msgs.msg import Joy
from roslib.msg import Header
return Joy(Header(), [0.1, 0.2, 0.3, 0.4, 0.5], [0, 1, 0, 1, 0])
def start(self):
self.proxy.start()
self.ndof_finger = 3
self.flex_factor_index = [0.3] * self.ndof_finger
self.flex_factor_ring = [0.3] * self.ndof_finger
self.flex_factor_pinky = [0.3] * self.ndof_finger
self.flex_factor_thumb = [0.3] * 2
self.p = subprocess.Popen(
['roslaunch', 'm3_defs_ros', 'm3_launch.launch'])
rospy.init_node("joint_state_publisher")
self.pub = rospy.Publisher("/joint_states", JointState)
time.sleep(4.0)
hand_names = self.proxy.get_available_components('m3hand')
self.hands = []
self.hand_nums = []
for i in range(len(hand_names)):
self.hands.append(m3f.create_component(hand_names[i]))
self.proxy.subscribe_status(self.hands[i])
#self.proxy.publish_command(self.hands[i])
if hand_names[i][-2].isdigit():
self.hand_nums.append(hand_names[i][-2:])
else:
self.hand_nums.append(hand_names[i][-1])
#r_hand_ua_num = 14
self.ndof_hand_ua = 12
self.positions = []
self.joints = []
for j in range(len(self.hands)):
for i in range(self.ndof_hand_ua):
self.positions.append(0.0)
self.joints.append('m3joint_ua_mh' + str(self.hand_nums[j]) +
'_j' + str(i))
# Thumb: J0,J1,J2
# Index: J3, J4, J5
# Ring: J6,J7,J8
# Pinkie: J9, J10, J11
print 'Starting hand viz.'
while (True):
self.positions = []
self.proxy.step()
for i in range(len(self.hands)):
th = self.hands[i].get_theta_rad()
#Thumb
self.positions.append(-th[0] + 1.57) #0
self.positions.append(th[1] * self.flex_factor_thumb[0])
self.positions.append(th[1] * self.flex_factor_thumb[1])
#Index
self.positions.append(th[2] * self.flex_factor_index[0])
self.positions.append(th[2] * self.flex_factor_index[1])
self.positions.append(th[2] * self.flex_factor_index[2])
#Ring
self.positions.append(th[3] * self.flex_factor_ring[0])
self.positions.append(th[3] * self.flex_factor_ring[1])
self.positions.append(th[3] * self.flex_factor_ring[2])
#Pinkie
self.positions.append(th[4] * self.flex_factor_pinky[0])
self.positions.append(th[4] * self.flex_factor_pinky[1])
self.positions.append(th[4] * self.flex_factor_pinky[2])
if self.pub is not None and not rospy.is_shutdown():
header = Header(0, rospy.Time.now(), '0')
self.pub.publish(
JointState(header, self.joints, self.positions,
[0] * len(self.positions),
[0] * len(self.positions)))
else:
print 'Error...exiting.'
break
time.sleep(0.1)
def step_ros(self):
r_arm = 'right_arm'
l_arm = 'left_arm'
self.cb_lock.acquire()
r_jep = copy.copy(self.r_jep)
l_jep = copy.copy(self.l_jep)
r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
self.cb_lock.release()
self.set_jep(r_arm, r_jep)
self.set_jep(l_arm, l_jep)
self.set_alpha(r_arm, r_alpha)
self.set_alpha(l_arm, l_alpha)
self.step()
motor_pwr_state = self.is_motor_power_on()
if not motor_pwr_state:
self.maintain_configuration()
q_r = self.get_joint_angles(r_arm)
q_l = self.get_joint_angles(l_arm)
if self.floating_arms:
if self.qr_prev == None or self.floating_arms_counter < 100:
self.qr_prev = q_r
self.ql_prev = q_l
self.floating_arms_counter += 1
else:
tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
r_arr = np.array(q_r)
l_arr = np.array(q_l)
prev_r_arr = np.array(self.qr_prev)
prev_l_arr = np.array(self.ql_prev)
dqr = np.array(q_r) - np.array(prev_r_arr)
dql = np.array(q_l) - np.array(prev_l_arr)
dqr = dqr * (np.abs(dqr) > tol)
dql = dql * (np.abs(dql) > tol)
r_jep = (np.array(r_jep) + dqr).tolist()
l_jep = (np.array(l_jep) + dql).tolist()
self.cb_lock.acquire()
self.r_jep = copy.copy(r_jep)
self.l_jep = copy.copy(l_jep)
self.cb_lock.release()
change_idxs = np.where(dqr != 0)
prev_r_arr[change_idxs] = r_arr[change_idxs]
change_idxs = np.where(dql != 0)
prev_l_arr[change_idxs] = l_arr[change_idxs]
self.qr_prev = prev_r_arr.tolist()
self.ql_prev = prev_l_arr.tolist()
f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
f_r = self.xhat_force[r_arm]
f_l = self.xhat_force[l_arm]
# publish stuff over ROS.
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
self.q_r_pub.publish(FloatArray(h, q_r))
self.q_l_pub.publish(FloatArray(h, q_l))
self.jep_r_pub.publish(FloatArray(h, r_jep))
self.jep_l_pub.publish(FloatArray(h, l_jep))
self.alph_r_pub.publish(FloatArray(h, r_alpha))
self.alph_l_pub.publish(FloatArray(h, l_alpha))
h.frame_id = '/torso_lift_link'
nms = self.joint_names_list['right_arm'] + self.joint_names_list[
'left_arm']
pos = q_r + q_l
self.joint_state_pub.publish(
JointState(h, nms, pos, [0.] * len(pos), [0.] * len(pos)))
h.frame_id = ar.link_tf_name(r_arm, 7)
self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
self.force_r_pub.publish(FloatArray(h, f_r))
h.frame_id = ar.link_tf_name(l_arm, 7)
self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
self.force_l_pub.publish(FloatArray(h, f_l))
self.pwr_state_pub.publish(Bool(motor_pwr_state))
rospy.logout('Sleeping ...')
rospy.sleep(1.0)
rospy.logout('... begin')
r_arm = 'right_arm'
l_arm = 'left_arm'
transform_bcast = tfb.TransformBroadcaster()
torso_link_name = ar.link_tf_name(r_arm, 0)
while not rospy.is_shutdown():
rospy.sleep(0.1)
f_r = arm_client.get_wrist_force(r_arm, base_frame=True)
f_l = arm_client.get_wrist_force(l_arm, base_frame=True)
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
force_r_pub.publish(FloatArray(h, f_r))
force_l_pub.publish(FloatArray(h, f_l))
publish_sphere_marker((0.5,0.5,0.5), 0.08, torso_link_name,
time_stamp, 'both_arms', 0)
for arm in [r_arm, l_arm]:
q = arm_client.get_joint_angles(arm)
links = [2, 3, 7]
for i in links:
p, rot = arms.FK_all(arm, q, i)
qaut = tr.matrix_to_quaternion(rot)
frameid = ar.link_tf_name(arm, i)
transform_bcast.sendTransform(p.A1.tolist(), qaut, time_stamp,
from manipulation_transforms.srv import *
from roslib.msg import Header
if __name__ == "__main__":
rospy.init_node(PKG)
ns = "/cart_pushing/"
#ns = ""
service_name = ns + PKG + "_server/SetInitialTransforms"
rospy.loginfo("Waiting for service \"%s\"" % service_name)
rospy.wait_for_service(service_name)
try:
SetInitialTransforms_srv = rospy.ServiceProxy(service_name,
SetInitialTransforms)
obj_init_pose = PoseStamped(header=Header(frame_id="base_footprint",
stamp=rospy.Time.now()),
pose=Pose(position=Point(0.5, 0.0, 0.344),
orientation=Quaternion(
-0.5, -0.44, -0.45, 0.59)))
gripper_init_poses = []
gripper_init_poses.append(
PoseStamped(header=Header(frame_id="base_footprint",
stamp=rospy.Time.now()),
pose=Pose(position=Point(0.5, -0.12, 0.344),
orientation=Quaternion(
-0.5, -0.44, -0.45, 0.59))))
gripper_init_poses.append(
PoseStamped(header=Header(frame_id="base_footprint",
stamp=rospy.Time.now()),
def step_ros(self):
r_arm = 'right_arm'
l_arm = 'left_arm'
self.cb_lock.acquire()
r_jep = copy.copy(self.r_jep)
l_jep = copy.copy(self.l_jep)
r_alpha = copy.copy(self.arm_settings['right_arm'].stiffness_list)
l_alpha = copy.copy(self.arm_settings['left_arm'].stiffness_list)
self.cb_lock.release()
self.set_jep(r_arm, r_jep)
self.set_jep(l_arm, l_jep)
self.set_alpha(r_arm, r_alpha)
self.set_alpha(l_arm, l_alpha)
self.step()
motor_pwr_state = self.is_motor_power_on()
if not motor_pwr_state:
self.maintain_configuration()
q_r = self.get_joint_angles(r_arm)
q_l = self.get_joint_angles(l_arm)
if self.floating_arms:
if self.qr_prev == None or self.floating_arms_counter < 100:
self.qr_prev = q_r
self.ql_prev = q_l
self.floating_arms_counter += 1
else:
tol = np.radians([5., 2., 10., 2., 10., 0.03, 0.6])
r_arr = np.array(q_r)
l_arr = np.array(q_l)
prev_r_arr = np.array(self.qr_prev)
prev_l_arr = np.array(self.ql_prev)
dqr = np.array(q_r) - np.array(prev_r_arr)
dql = np.array(q_l) - np.array(prev_l_arr)
dqr = dqr * (np.abs(dqr) > tol)
dql = dql * (np.abs(dql) > tol)
r_jep = (np.array(r_jep) + dqr).tolist()
l_jep = (np.array(l_jep) + dql).tolist()
self.cb_lock.acquire()
self.r_jep = copy.copy(r_jep)
self.l_jep = copy.copy(l_jep)
self.cb_lock.release()
change_idxs = np.where(dqr != 0)
prev_r_arr[change_idxs] = r_arr[change_idxs]
change_idxs = np.where(dql != 0)
prev_l_arr[change_idxs] = l_arr[change_idxs]
self.qr_prev = prev_r_arr.tolist()
self.ql_prev = prev_l_arr.tolist()
f_raw_r = self.get_wrist_force(r_arm).A1.tolist()
f_raw_l = self.get_wrist_force(l_arm).A1.tolist()
f_r = self.xhat_force[r_arm]
f_l = self.xhat_force[l_arm]
# publish stuff over ROS.
time_stamp = rospy.Time.now()
h = Header()
h.stamp = time_stamp
self.q_r_pub.publish(FloatArray(h, q_r))
self.q_l_pub.publish(FloatArray(h, q_l))
self.jep_r_pub.publish(FloatArray(h, r_jep))
self.jep_l_pub.publish(FloatArray(h, l_jep))
self.alph_r_pub.publish(FloatArray(h, r_alpha))
self.alph_l_pub.publish(FloatArray(h, l_alpha))
h.frame_id = '/torso_lift_link'
nms = self.joint_names_list['right_arm'] + self.joint_names_list['left_arm']
pos = q_r + q_l
self.joint_state_pub.publish(JointState(h, nms, pos,
[0.]*len(pos), [0.]*len(pos)))
h.frame_id = ar.link_tf_name(r_arm, 7)
self.force_raw_r_pub.publish(FloatArray(h, f_raw_r))
self.force_r_pub.publish(FloatArray(h, f_r))
h.frame_id = ar.link_tf_name(l_arm, 7)
self.force_raw_l_pub.publish(FloatArray(h, f_raw_l))
self.force_l_pub.publish(FloatArray(h, f_l))
self.pwr_state_pub.publish(Bool(motor_pwr_state))