def __init__(self, simulation, namespace='', node=True):
self.namespace = namespace
# give possibility to use the interface directly as class with setting node=False
if node:
if namespace == '':
rospy.init_node("pybullet_sim")
else:
rospy.init_node('pybullet_sim',
anonymous=True,
argv=['clock:=/' + self.namespace + '/clock'])
self.simulation = simulation
self.namespace = namespace
self.last_time = time.time()
self.last_linear_vel = (0, 0, 0)
# messages
self.real_time_msg = Float32()
self.joint_state_msg = JointState()
self.joint_state_msg.header.frame_id = "base_link"
self.joint_state_msg.name = self.simulation.get_joint_names()
self.imu_msg = Imu()
self.imu_msg.header.frame_id = "imu_frame"
self.clock_msg = Clock()
self.foot_msg_left = FootPressure()
self.foot_msg_left.header.frame_id = 'l_sole'
self.foot_msg_right = FootPressure()
self.foot_msg_right.header.frame_id = 'r_sole'
self.odom_msg = Odometry()
self.odom_msg.header.frame_id = "odom"
self.odom_msg.child_frame_id = "base_link"
# we just use the first robot
self.robot_index = self.simulation.robot_indexes[0]
srv = Server(simConfig,
self._dynamic_reconfigure_callback,
namespace=namespace)
# publisher
self.left_foot_pressure_publisher = rospy.Publisher(
self.namespace + "foot_pressure_left/raw",
FootPressure,
queue_size=1)
self.right_foot_pressure_publisher = rospy.Publisher(
self.namespace + "foot_pressure_right/raw",
FootPressure,
queue_size=1)
self.left_foot_pressure_publisher_filtered = rospy.Publisher(
self.namespace + "foot_pressure_left/filtered",
FootPressure,
queue_size=1)
self.right_foot_pressure_publisher_filtered = rospy.Publisher(
self.namespace + "foot_pressure_right/filtered",
FootPressure,
queue_size=1)
self.joint_publisher = rospy.Publisher(self.namespace + "joint_states",
JointState,
queue_size=1)
self.imu_publisher = rospy.Publisher(self.namespace + "imu/data",
Imu,
queue_size=1)
self.clock_publisher = rospy.Publisher(self.namespace + "clock",
Clock,
queue_size=1)
self.real_time_factor_publisher = rospy.Publisher(self.namespace +
"real_time_factor",
Float32,
queue_size=1)
self.true_odom_publisher = rospy.Publisher(self.namespace +
"true_odom",
Odometry,
queue_size=1)
self.cop_l_pub_ = rospy.Publisher(self.namespace + "cop_l",
PointStamped,
queue_size=1)
self.cop_r_pub_ = rospy.Publisher(self.namespace + "cop_r",
PointStamped,
queue_size=1)
# subscriber
self.joint_goal_subscriber = rospy.Subscriber(
self.namespace + "DynamixelController/command",
JointCommand,
self.joint_goal_cb,
queue_size=1,
tcp_nodelay=True)
self.reset_subscriber = rospy.Subscriber(self.namespace + "reset",
Bool,
self.reset_cb,
queue_size=1,
tcp_nodelay=True)
b = tkinter.Button(master, command=zero, text="Zero")
b.pack()
rclpy.init(args=None)
pub_r = self.create_publisher(FootPressure,
"/foot_pressure_right/raw",
1,
tcp_nodelay=True)
pub_l = self.create_publisher(FootPressure,
"/foot_pressure_left/raw",
1,
tcp_nodelay=True)
rate = self.create_rate(args.rate)
msg_l = FootPressure()
msg_r = FootPressure()
def publish(timer):
msg_l.header.stamp = msg_r.header.stamp = rospy.get_rostime()
msg_l.left_back = zeroes[0] + scales[0] * force_values[0] + random.random(
) * args.noise
msg_l.left_front = zeroes[1] + scales[1] * force_values[1] + random.random(
) * args.noise
msg_l.right_back = zeroes[2] + scales[2] * force_values[2] + random.random(
) * args.noise
msg_l.right_front = zeroes[
3] + scales[3] * force_values[3] + random.random() * args.noise
msg_r.left_back = zeroes[4] + scales[4] * force_values[4] + random.random(
def get_pressure_message(self):
current_time = rospy.Time.from_sec(self.time)
left_pressure = FootPressure()
left_pressure.header.stamp = current_time
left_pressure.left_back = self.pressure_sensors[0].getValues()[2]
left_pressure.left_front = self.pressure_sensors[1].getValues()[2]
left_pressure.right_front = self.pressure_sensors[2].getValues()[2]
left_pressure.right_back = self.pressure_sensors[3].getValues()[2]
right_pressure = FootPressure()
left_pressure.header.stamp = current_time
right_pressure.left_back = self.pressure_sensors[4].getValues()[2]
right_pressure.left_front = self.pressure_sensors[5].getValues()[2]
right_pressure.right_front = self.pressure_sensors[6].getValues()[2]
right_pressure.right_back = self.pressure_sensors[7].getValues()[2]
# compute center of pressures of the feet
pos_x = 0.085
pos_y = 0.045
# we can take a very small threshold, since simulation gives more accurate values than reality
threshold = 1
cop_l = PointStamped()
cop_l.header.frame_id = self.l_sole_frame
cop_l.header.stamp = current_time
sum = left_pressure.left_back + left_pressure.left_front + left_pressure.right_front + left_pressure.right_back
if sum > threshold:
cop_l.point.x = (left_pressure.left_front +
left_pressure.right_front -
left_pressure.left_back -
left_pressure.right_back) * pos_x / sum
cop_l.point.x = max(min(cop_l.point.x, pos_x), -pos_x)
cop_l.point.y = (left_pressure.left_front + left_pressure.left_back
- left_pressure.right_front -
left_pressure.right_back) * pos_y / sum
cop_l.point.y = max(min(cop_l.point.x, pos_y), -pos_y)
else:
cop_l.point.x = 0
cop_l.point.y = 0
cop_r = PointStamped()
cop_r.header.frame_id = self.r_sole_frame
cop_r.header.stamp = current_time
sum = right_pressure.right_back + right_pressure.right_front + right_pressure.right_front + right_pressure.right_back
if sum > threshold:
cop_r.point.x = (right_pressure.left_front +
right_pressure.right_front -
right_pressure.left_back -
right_pressure.right_back) * pos_x / sum
cop_r.point.x = max(min(cop_r.point.x, pos_x), -pos_x)
cop_r.point.y = (right_pressure.left_front +
right_pressure.left_back -
right_pressure.right_front -
right_pressure.right_back) * pos_y / sum
cop_r.point.y = max(min(cop_r.point.x, pos_y), -pos_y)
else:
cop_r.point.x = 0
cop_r.point.y = 0
return left_pressure, right_pressure, cop_l, cop_r
def get_pressure_right(self):
self.node.get_logger().warn("pressure method not implemented",
once=True)
return FootPressure()
def handle_force_measurements(self, forces):
if not forces:
return
data = {}
for force in forces:
name = force.name
if name in self.force3d_sensors:
data[name] = force.value
else:
rospy.logwarn(f"Unknown force measurement: '{name}'",
logger_name="rc_api")
left_pressure_msg = FootPressure()
left_pressure_msg.header.stamp = self.stamp
# TODO: Frame IDs
left_pressure_msg.left_back = data['llb']
left_pressure_msg.left_front = data['llf']
left_pressure_msg.right_front = data['lrf']
left_pressure_msg.right_back = data['lrb']
right_pressure_msg = FootPressure()
right_pressure_msg.header.stamp = self.stamp
# TODO: Frame IDs
right_pressure_msg.left_back = data['rlb']
right_pressure_msg.left_front = data['rlf']
right_pressure_msg.right_front = data['rrf']
right_pressure_msg.right_back = data['rrb']
# compute center of pressures of the feet
pos_x = 0.085
pos_y = 0.045
# we can take a very small threshold, since simulation gives more accurate values than reality
threshold = 1
cop_l_msg = PointStamped()
cop_l_msg.header.stamp = self.stamp
cop_l_msg.header.frame_id = self.l_sole_frame
sum = left_pressure_msg.left_back + left_pressure_msg.left_front + left_pressure_msg.right_front + left_pressure_msg.right_back
if sum > threshold:
cop_l_msg.point.x = (left_pressure_msg.left_front +
left_pressure_msg.right_front -
left_pressure_msg.left_back -
left_pressure_msg.right_back) * pos_x / sum
cop_l_msg.point.x = max(min(cop_l_msg.point.x, pos_x), -pos_x)
cop_l_msg.point.y = (left_pressure_msg.left_front +
left_pressure_msg.left_back -
left_pressure_msg.right_front -
left_pressure_msg.right_back) * pos_y / sum
cop_l_msg.point.y = max(min(cop_l_msg.point.x, pos_y), -pos_y)
else:
cop_l_msg.point.x = 0
cop_l_msg.point.y = 0
cop_r_msg = PointStamped()
cop_r_msg.header.stamp = self.stamp
cop_r_msg.header.frame_id = self.r_sole_frame
sum = right_pressure_msg.right_back + right_pressure_msg.right_front + right_pressure_msg.right_front + right_pressure_msg.right_back
if sum > threshold:
cop_r_msg.point.x = (right_pressure_msg.left_front +
right_pressure_msg.right_front -
right_pressure_msg.left_back -
right_pressure_msg.right_back) * pos_x / sum
cop_r_msg.point.x = max(min(cop_r_msg.point.x, pos_x), -pos_x)
cop_r_msg.point.y = (right_pressure_msg.left_front +
right_pressure_msg.left_back -
right_pressure_msg.right_front -
right_pressure_msg.right_back) * pos_y / sum
cop_r_msg.point.y = max(min(cop_r_msg.point.x, pos_y), -pos_y)
else:
cop_r_msg.point.x = 0
cop_r_msg.point.y = 0
self.pub_pressure_left.publish(left_pressure_msg)
self.pub_pressure_right.publish(right_pressure_msg)
self.pub_cop_l.publish(cop_l_msg)
self.pub_cop_r_.publish(cop_r_msg)