From ea2feef15170f6b17fe3fd56ffe665f9c136338e Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 20 Jul 2017 11:40:46 +0900 Subject: [PATCH] Apply flipped to joint state in joint_position_controller --- .../joint_position_controller.py | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py index bb3a637..d25ae6e 100755 --- a/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py +++ b/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py @@ -172,9 +172,14 @@ def process_motor_states(self, state_list): self.joint_state.motor_temps = [state.temperature] self.joint_state.goal_pos = self.raw_to_rad(state.goal, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK) self.joint_state.current_pos = self.raw_to_rad(state.position, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK) - self.joint_state.error = state.error * self.RADIANS_PER_ENCODER_TICK - self.joint_state.velocity = state.speed * self.VELOCITY_PER_TICK - self.joint_state.load = state.load + if self.flipped: + self.joint_state.error = -state.error * self.RADIANS_PER_ENCODER_TICK + self.joint_state.velocity = -state.speed * self.VELOCITY_PER_TICK + self.joint_state.load = -state.load + else: + self.joint_state.error = state.error * self.RADIANS_PER_ENCODER_TICK + self.joint_state.velocity = state.speed * self.VELOCITY_PER_TICK + self.joint_state.load = state.load self.joint_state.is_moving = state.moving self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)