def state_msg(self, ref_state, type_msg):
if type_msg == 'state':
st = JointTrajectoryControllerState()
elif type_msg == 'feedback':
st = FollowJointTrajectoryFeedback()
st.joint_names = [self.gripper_joint_name]
st.header = ref_state.header
st.desired.positions.extend(
[p * 2.0 for p in ref_state.desired.positions[:1]])
st.desired.velocities.extend(ref_state.desired.velocities[:1])
st.desired.accelerations.extend(ref_state.desired.accelerations[:1])
st.desired.effort.extend(ref_state.desired.effort[:1])
st.desired.time_from_start = ref_state.desired.time_from_start
st.actual.positions.extend(
[p * 2.0for p in ref_state.actual.positions[:1]])
st.actual.velocities.extend(ref_state.actual.velocities[:1])
st.actual.accelerations.extend(ref_state.actual.accelerations[:1])
st.actual.effort.extend(ref_state.actual.effort[:1])
st.actual.time_from_start = ref_state.actual.time_from_start
st.error.positions.extend(
[p * 2.0 for p in ref_state.error.positions[:1]])
st.error.velocities.extend(ref_state.error.velocities[: 1])
st.error.accelerations.extend(ref_state.error.accelerations[: 1])
st.error.effort.extend(ref_state.error.effort[: 1])
return st
def state_msg(self, ref_state, type_msg):
if type_msg == 'state':
st = JointTrajectoryControllerState()
elif type_msg == 'feedback':
st = FollowJointTrajectoryFeedback()
st.joint_names = [self.gripper_joint_name]
st.header = ref_state.header
st.desired.positions.extend(
[p * 2.0 for p in ref_state.desired.positions[:1]])
st.desired.velocities.extend(ref_state.desired.velocities[:1])
st.desired.accelerations.extend(ref_state.desired.accelerations[:1])
st.desired.effort.extend(ref_state.desired.effort[:1])
st.desired.time_from_start = ref_state.desired.time_from_start
st.actual.positions.extend(
[p * 2.0 for p in ref_state.actual.positions[:1]])
st.actual.velocities.extend(ref_state.actual.velocities[:1])
st.actual.accelerations.extend(ref_state.actual.accelerations[:1])
st.actual.effort.extend(ref_state.actual.effort[:1])
st.actual.time_from_start = ref_state.actual.time_from_start
st.error.positions.extend(
[p * 2.0 for p in ref_state.error.positions[:1]])
st.error.velocities.extend(ref_state.error.velocities[:1])
st.error.accelerations.extend(ref_state.error.accelerations[:1])
st.error.effort.extend(ref_state.error.effort[:1])
return st
def js_cb(self, js_msg):
"""
:type js_msg: JointState
"""
try:
torso_offset = js_msg.name.index(self.torso_joint_name)
except ValueError:
rospy.logerr("No {} in omnidrive/joint_states. Can't get position.".format(self.torso_joint_name))
return
state_msg = JointTrajectoryControllerState()
state_msg.joint_names = [self.torso_joint_name]
self.state_pub.publish(state_msg)
def default(self, ci='unused'):
js = JointTrajectoryControllerState()
js.header = self.get_ros_header()
# timestamp is not a joint
joints = dict(self.data)
del joints['timestamp']
js.joint_names = joints.keys()
js.actual.positions = joints.values()
js.actual.velocities = [0.0] * len(joints)
js.actual.accelerations = [0.0] * len(joints)
self.publish(js)
def test_JointTrajectoryControllerState():
'''
PUBLISHER METHODE: JointControllerStates
'''
pub_JointControllerStates = rospy.Publisher('state', JointTrajectoryControllerState, queue_size=10)
message = JointTrajectoryControllerState()
message.header.frame_id = "testID: 101"
message .joint_names = ["fl_caster_r_wheel_joint", "bl_caster_r_wheel_joint", "br_caster_r_wheel_joint", "fr_caster_r_wheel_joint", "fl_caster_rotation_joint", "bl_caster_rotation_joint", "br_caster_rotation_joint", "fr_caster_rotation_joint"]
message.actual.velocities = [4,4,4,4,4,4,4,4]
message.actual.positions = [4,4,4,4,4,4,4,4]
print "JointControllerStates: " + message.header.frame_id
pub_JointControllerStates.publish(message)
def test_JointTrajectoryControllerState():
'''
PUBLISHER METHODE: JointControllerStates
'''
pub_JointControllerStates = rospy.Publisher('state',
JointTrajectoryControllerState,
queue_size=10)
message = JointTrajectoryControllerState()
message.header.frame_id = "testID: 101"
message.joint_names = [
"fl_caster_r_wheel_joint", "bl_caster_r_wheel_joint",
"br_caster_r_wheel_joint", "fr_caster_r_wheel_joint",
"fl_caster_rotation_joint", "bl_caster_rotation_joint",
"br_caster_rotation_joint", "fr_caster_rotation_joint"
]
message.actual.velocities = [4, 4, 4, 4, 4, 4, 4, 4]
message.actual.positions = [4, 4, 4, 4, 4, 4, 4, 4]
print "JointControllerStates: " + message.header.frame_id
pub_JointControllerStates.publish(message)
def js_cb(self, js):
self.current_pose = np.array([
js.position[self.x_index_js], js.position[self.y_index_js],
js.position[self.z_index_js]
])
try:
current_vel = np.array([
js.velocity[self.x_index_js], js.velocity[self.y_index_js],
js.velocity[self.z_index_js]
])
except IndexError as e:
rospy.logwarn('velocity entry in joint state is empty')
return
time = js.header.stamp.to_sec()
self.last_cmd = current_vel
if not self.done:
time_from_start = time - self.start_time
if time_from_start > 0:
goal_pose = self.pose_traj2(time_from_start)
cmd = make_cmd(self.current_pose, goal_pose, self.last_cmd,
self.goal_vel2, time_from_start)
if np.any(np.isnan(cmd)):
print('f**k')
else:
cmd = self.limit_vel(cmd)
cmd = kdl_to_np(
js_to_kdl(*self.current_pose).M.Inverse() *
make_twist(*cmd))
self.debug_vel.publish(np_to_msg(cmd))
# cmd = self.hack(cmd)
cmd_msg = np_to_msg(cmd)
self.cmd_vel_sub.publish(cmd_msg)
self.last_cmd = cmd
state = JointTrajectoryControllerState()
state.joint_names = [x_joint, y_joint, z_joint]
self.state_pub.publish(state)
def step(self, action):
state = []
reward = []
done = False
data_arm_state = None
while data_arm_state is None:
try:
data_arm_state = rospy.wait_for_message('/ariac/arm/state', JointTrajectoryControllerState, timeout=5)
except:
pass
data_arm_state2 = JointTrajectoryControllerState()
data_arm_state2.joint_names = data_arm_state.joint_names[1:]
data_arm_state2.desired.positions = data_arm_state.desired.positions[1:]
data_arm_state2.desired.velocities = data_arm_state.desired.velocities[1:]
data_arm_state2.desired.accelerations = data_arm_state.desired.accelerations[1:]
data_arm_state2.actual.positions = data_arm_state.actual.positions[1:]
data_arm_state2.actual.velocities = data_arm_state.actual.velocities[1:]
data_arm_state2.actual.accelerations = data_arm_state.actual.accelerations[1:]
data_arm_state2.error.positions = data_arm_state.error.positions[1:]
data_arm_state2.error.velocities = data_arm_state.error.velocities[1:]
data_arm_state2.error.accelerations = data_arm_state.error.accelerations[1:]
# Collect the end effector points and velocities in
# cartesian coordinates for the process_observationsstate.
# Collect the present joint angles and velocities from ROS for the state.
last_observations = self.process_observations(data_arm_state2, self.environment)
# # # Get Jacobians from present joint angles and KDL trees
# # # The Jacobians consist of a 6x6 matrix getting its from from
# # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw])
ee_link_jacobians = self.get_jacobians(last_observations)
if self.environment['link_names'][-1] is None:
print("End link is empty!!")
return None
else:
# print(self.environment['link_names'][-1])
trans, rot = forward_kinematics(self.scara_chain,
self.environment['link_names'],
last_observations[:self.scara_chain.getNrOfJoints()],
base_link=self.environment['link_names'][0],
end_link=self.environment['link_names'][-1])
# #
rotation_matrix = np.eye(4)
rotation_matrix[:3, :3] = rot
rotation_matrix[:3, 3] = trans
# angle, dir, _ = rotation_from_matrix(rotation_matrix)
# #
# current_quaternion = np.array([angle]+dir.tolist())#
# I need this calculations for the new reward function, need to send them back to the run scara or calculate them here
current_quaternion = quaternion_from_matrix(rotation_matrix)
current_ee_tgt = np.ndarray.flatten(get_ee_points(self.environment['end_effector_points'],
trans,
rot).T)
ee_points = current_ee_tgt - self.realgoal#self.environment['ee_points_tgt']
ee_points_jac_trans, _ = self.get_ee_points_jacobians(ee_link_jacobians,
self.environment['end_effector_points'],
rot)
ee_velocities = self.get_ee_points_velocities(ee_link_jacobians,
self.environment['end_effector_points'],
rot,
last_observations)
# Concatenate the information that defines the robot state
# vector, typically denoted asrobot_id 'x'.
state = np.r_[np.reshape(last_observations, -1),
np.reshape(ee_points, -1),
np.reshape(ee_velocities, -1),]
self._pub.publish(self.get_trajectory_message([0.018231524968342513,
3.2196073949389703 + np.random.rand(1)[0] - .5,
-0.6542426695478509 + np.random.rand(1)[0] - .5,
1.7401498071871018 + np.random.rand(1)[0] - .5,
3.7354939354077596 + np.random.rand(1)[0] - .5,
-1.510707301072792 + np.random.rand(1)[0] - .5,
0.00014565660628829136 + np.random.rand(1)[0] - .5]))
data_vacuum_state = None
while data_vacuum_state is None:
try:
data_vacuum_state = rospy.wait_for_message('/ariac/gripper/state', VacuumGripperState, timeout=5)
except:
pass
data_camera = None
while data_camera is None:
try:
data_camera = rospy.wait_for_message('/ariac/logical_camera_1', LogicalCameraImage, timeout=5)
except:
pass
try:
self.vacuum_gripper_control(enable=bool(state[0]))
except (rospy.ServiceException) as e:
print ("/gazebo/reset_simulation service call failed")
state = [data_arm_state, data_camera, data_vacuum_state]
return state, reward, done, {}
def js_cb(self, data):
msg = JointTrajectoryControllerState()
msg.joint_names = data.name
self.state_pub.publish(msg)