Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: arebgun/dynamixel_motor
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: master
Choose a base ref
...
head repository: smARTLab-liv/dynamixel_motor
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: master
Choose a head ref
Able to merge. These branches can be automatically merged.
  • 3 commits
  • 1 file changed
  • 2 contributors

Commits on Apr 20, 2016

  1. add dual motor support

    Youbot Sarah committed Apr 20, 2016
    Copy the full SHA
    8027fce View commit details

Commits on Mar 3, 2017

  1. add dual motor support

    Youbot Sarah authored and daenny committed Mar 3, 2017
    Copy the full SHA
    ce18355 View commit details
  2. Copy the full SHA
    122760c View commit details
Showing with 16 additions and 12 deletions.
  1. +16 −12 dynamixel_controllers/src/dynamixel_controllers/joint_position_controller_dual_motor.py
Original file line number Diff line number Diff line change
@@ -49,7 +49,8 @@
from dynamixel_driver.dynamixel_const import *
from dynamixel_controllers.joint_controller import JointController

from dynamixel_msgs.msg import JointState
# from dynamixel_msgs.msg import JointState
from slaw_msgs.msg import JointDualState

class JointPositionControllerDual(JointController):
def __init__(self, dxl_io, controller_namespace, port_namespace):
@@ -65,8 +66,10 @@ def __init__(self, dxl_io, controller_namespace, port_namespace):

self.flipped = self.master_min_angle_raw > self.master_max_angle_raw

self.joint_state = JointState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
self.joint_state = JointDualState(name=self.joint_name, motor_ids=[self.master_id, self.slave_id])
self.joint_state_publisher = rospy.Publisher(self.controller_namespace + '/state', JointDualState)


def initialize(self):
# verify that the expected motor is connected and responding
available_ids = rospy.get_param('dynamixel/%s/connected_ids' % self.port_namespace, [])
@@ -183,16 +186,17 @@ def process_motor_states(self, state_list):
if state.id in [self.master_id, self.slave_id]: states[state.id] = state

if self.master_id in states and self.slave_id in states:
state = states[self.master_id]
self.joint_state.motor_temps = [state.temperature, states[self.slave_id].temperature]
self.joint_state.goal_pos = self.raw_to_rad(state.goal, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)
self.joint_state.current_pos = self.raw_to_rad(state.position, self.master_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
self.joint_state.is_moving = state.moving
self.joint_state.header.stamp = rospy.Time.from_sec(state.timestamp)
self.joint_state_pub.publish(self.joint_state)
master_state = states[self.master_id]
slave_state = states[self.slave_id]
self.joint_state.motor_temps = [master_state.temperature, slave_state.temperature]
self.joint_state.goal_pos = [self.raw_to_rad(master_state.goal, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK), self.raw_to_rad(slave_state.goal, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)]
self.joint_state.current_pos = [self.raw_to_rad(master_state.position, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK), self.raw_to_rad(slave_state.position, self.master_initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)]
self.joint_state.error = [master_state.error * self.RADIANS_PER_ENCODER_TICK, slave_state.error * self.RADIANS_PER_ENCODER_TICK]
self.joint_state.velocity = [master_state.speed * self.VELOCITY_PER_TICK, slave_state.speed * self.VELOCITY_PER_TICK]
self.joint_state.load = [master_state.load, slave_state.load]
self.joint_state.is_moving = [master_state.moving, slave_state.moving]
self.joint_state.header.stamp = rospy.Time.from_sec(master_state.timestamp)
self.joint_state_publisher.publish(self.joint_state)

def process_command(self, msg):
angle = msg.data