def get_hand_joint_states(self, all_poses):
"""Get joint states for both hand.
:param all_poses: PoseArray Poses of all segment. Derived from @get_all_poses
:return: JointState/None JointState/None.
Left hand joint states (10), right hand joint states (10)
"""
if self.header is None or not self.header.is_valid:
return None, None
if self.header.finger_segments_num != 40:
rospy.logwarn_throttle(
3, "Finger segment number is not 40: {}".format(
self.header.finger_segments_num))
return None, None
left_hand_js = SensorMsg.JointState()
right_hand_js = SensorMsg.JointState()
left_hand_js.header = all_poses.header
right_hand_js.header = all_poses.header
# Here the hand joint states are defined as j1 and j2 for thumb to pinky
# j1 is the angle between metacarpals (parent) and proximal phalanges (child)
# j2 is the angle between proximal phalanges and intermediate/distal phalanges (distal is only for thumb)
base_num = self.header.body_segments_num + self.header.props_num
for i in [1, 4, 8, 12, 16]:
meta_id = base_num + i
left_hand_js.position.extend(
self._get_finger_j1_j2(all_poses.poses, meta_id))
for i in [1, 4, 8, 12, 16]:
meta_id = base_num + 20 + i
right_hand_js.position.extend(
self._get_finger_j1_j2(all_poses.poses, meta_id))
return left_hand_js, right_hand_js
def __init__(self):
self.odom = None
# Range translation Rapiro -> ROS
self.range_msg = sensor.Range()
self.range_msg.header.frame_id = 'usr_frame'
self.range_msg.radiation_type = sensor.Range.ULTRASOUND
self.range_msg.field_of_view = math.radians(45.0)
self.range_msg.min_range = 0.02
self.range_msg.max_range = 4.0
self.range_pub = rospy.Publisher('range', sensor.Range, queue_size=10)
self.range_sub = rospy.Subscriber('range_raw', rapiro.Range,
self.range_cb)
# JointState translation Rapiro -> ROS
self.joint_state_msg = sensor.JointState()
self.joint_state_msg.name = [
self.joint_names[id] for id in range(rapiro.JointIDs.N_JONTS)
]
self.joint_state_pub = rospy.Publisher('joint_states',
sensor.JointState,
queue_size=10)
self.joint_state_sub = rospy.Subscriber('joint_states_raw',
rapiro.JointState,
self.joint_state_cb)
# JointTrajectroy translation ROS -> Rapiro
self.active_trajectory = None
self.trajectory_msg = None
self.trajectory_pub = rospy.Publisher('traj_cmd_raw',
rapiro.Trajectory,
queue_size=10)
self.trajectory_sub = rospy.Subscriber('traj_cmd',
traj.JointTrajectory,
self.trajectory_cb)
def _writeMessages(self, Bag, Data):
# Create a a list of messages from joints [num_samples x num_joints].
# That's one message per position.
numJoints = self.Data.joints().shape[1]
messages = []
for pdx in range(len(Data.joints())):
js = rosmsg.JointState()
js.name = map(lambda x: "joint_%d" % (x + 1), range(numJoints))
#js.position = self.Data.Joints[pdx].tolist()
js.position = self.joints()[pdx].tolist()
messages.append(js)
for msg in messages:
Bag.write(self._TopicName, msg)
def move_to_target_client(value):
# Creates the SimpleActionClient, passing the type of the action
# (MoveToTargetAction) to the constructor.
client = actionlib.SimpleActionClient('/move_to_target',
thorp_msgs.MoveToTargetAction)
# Waits until the action server has started up and started listening for goals
client.wait_for_server()
# Depending on value's type, create a goal to send to the action server.
if type(value) is list:
target = sensor_msgs.JointState()
target.name = [
'arm_shoulder_pan_joint', 'arm_shoulder_lift_joint',
'arm_elbow_flex_joint', 'arm_wrist_flex_joint'
]
target.position = value
goal = thorp_msgs.MoveToTargetGoal(
target_type=thorp_msgs.MoveToTargetGoal.JOINT_STATE,
joint_state=target)
elif type(value) is str:
goal = thorp_msgs.MoveToTargetGoal(
target_type=thorp_msgs.MoveToTargetGoal.NAMED_TARGET,
named_target=value)
elif type(value) is tuple:
target = geometry_msgs.PoseStamped()
target.pose.position.x = value[0]
target.pose.position.y = value[1]
target.pose.position.z = value[2]
target.header.frame_id = 'arm_shoulder_lift_servo_link'
goal = thorp_msgs.MoveToTargetGoal(
target_type=thorp_msgs.MoveToTargetGoal.POSE_TARGET,
pose_target=target)
else:
print "Invalid target value: ", value
return None
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
# Prints out the result of executing the action
return client.get_result()
def _update_outputs(self, result):
"""
Calls callbacks, publishes to topics and to tf.
This class extends the MocapTracker implementation so it can publish the joint states on
the topic joint_states_topic, in addition to returning the computed state directly and via
callbacks.
Args:
result: dict - the outputs from one step of this tracker
"""
# Call superclass implementation so callbacks are handled properly
super(MocapUkfTracker, self)._update_outputs(result)
# Publish joint states
if self._estimation_pub is not None:
msg = sensor_msgs.JointState(position=result['mean'].squeeze(),
header=Header(stamp=rospy.Time.now()))
self._estimation_pub.publish(msg)
def __init__(self):
rospy.init_node('controller', anonymous=True)
self.rate = 100 #[Hz]
self.joy_val = cmsg.JointValues()
self.joint_states = smsg.JointState()
self.joy_switch = []
self.bias_current_scaling = 700.0
self.internal_impedance = 500.0
self.Kmode = False
self.time_switch_last = rospy.get_rostime()
self.initial_position_boom = 0.0
self.sub_joy_right = rospy.Subscriber('joy_right', smsg.Joy,
self.cb_joy_right)
self.sub_joy = rospy.Subscriber('joy_values', cmsg.JointValues,
self.cb_joy)
self.sub_joints_EPOS = rospy.Subscriber('/joint_states_EPOS',
smsg.JointState, self.cb_state)
self.pub_com = rospy.Publisher('commands',
cmsg.JointCommand,
queue_size=10)
def run(self):
# Get the base marker indices
base_indices = []
base_joint = self.kin_tree.get_root_joint()
for child in base_joint.children:
if not hasattr(child, 'children'):
# This is a feature
base_indices.append(int(child.name.split('_')[1]))
# Get all the marker indices of interest
all_marker_indices = []
for feature_name in self.kin_tree.get_features():
all_marker_indices.append(int(feature_name.split('_')[1]))
# Set the base coordinate transform for the mocap stream
desired = np.zeros((len(base_indices), 3, 1))
all_features = self.kin_tree.get_features()
for i, idx in enumerate(base_indices):
desired[i, :, 0] = all_features['mocap_' + str(idx)].q()
self.mocap_source.set_coordinates(base_indices,
desired,
mode='time_varying')
# Find the number of movable joints (so we know the dimension of the state space)
num_joints = len(self.kin_tree.get_twists())
# Create the observation and measurement models
test_ss_model = kinmodel.src.kinmodel.kinmodel.KinematicTreeStateSpaceModel(
self.kin_tree)
measurement_dim = len(
test_ss_model.measurement_model(np.zeros(num_joints)))
# Run the filter
for i, (frame, timestamp) in enumerate(self.mocap_source):
if self.exit:
break
feature_dict = {}
for marker_idx in all_marker_indices:
obs_point = kinmodel.src.kinmodel.kinmodel.new_geometric_primitive(
np.concatenate((frame[marker_idx, :, 0], np.ones(1))))
feature_dict['mocap_' + str(marker_idx)] = obs_point
if i == 0:
sys.stdout.flush()
initial_obs = test_ss_model.vectorize_measurement(feature_dict)
uk_filter = kinmodel.src.kinmodel.ukf.UnscentedKalmanFilter(
test_ss_model.process_model,
test_ss_model.measurement_model,
np.zeros(num_joints), # Initial state
np.identity(num_joints) * 0.25, # Initial error covariance
Q=math.pi / 2 / 80,
R=5e-3)
for i in range(50):
uk_filter.filter(initial_obs)
else:
# print('UKF Step: ' + str(i) + '/' + str(len(ukf_mocap)), end='\r')
# sys.stdout.flush()
obs_array = test_ss_model.vectorize_measurement(feature_dict)
joint_angles, covariance, squared_residual = uk_filter.filter(
obs_array, plot_error=False)
if self._callback is not None:
self._callback(i=i,
joint_angles=joint_angles,
covariance=covariance,
squared_residual=squared_residual)
if self._joint_states_pub is not None:
msg = sensor_msgs.JointState(
position=joint_angles.squeeze(),
header=Header(stamp=rospy.Time.now()))
self._joint_states_pub.publish(msg)
# Publish the base frame pose of the flexible object
if self._tf_pub is not None:
homog = np_la.inv(self.mocap_source.get_last_coordinates())
mocap_frame_name = self.mocap_source.get_frame_name()
if mocap_frame_name is not None:
self._tf_pub.sendTransform(
homog[0:3, 3],
tf.transformations.quaternion_from_matrix(homog),
rospy.Time.now(), '/object_base',
'/' + mocap_frame_name)
def test_make_topic_strings_with_array_builtin(self):
strings = ez_model.make_topic_strings(sen_msgs.JointState(),
'/joint_states')
self.assertEqual(len(strings), 8)
self.assertEqual('/joint_states/name[0]' in strings, True)
self.assertEqual('/joint_states/position[0]' in strings, True)