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: rethink-kmaroney/baxter_pykdl
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: master
Choose a base ref
...
head repository: RethinkRobotics/baxter_pykdl
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.
  • 7 commits
  • 3 files changed
  • 5 contributors

Commits on Jan 29, 2015

  1. Set package.xml to depend on liburdfdom-dev directly

    @IanTheEngineer
    IanTheEngineer committed Jan 29, 2015

    Verified

    This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
    GPG key ID: 4AEE18F83AFDEB23
    Expired
    Verified
    Learn about vigilant mode
    Copy the full SHA
    979081e View commit details

  2. Added dummy gravity elem to urdf parser

    @IanTheEngineer
    IanTheEngineer committed Jan 29, 2015
    Copy the full SHA
    9999dee View commit details

Commits on Jan 30, 2015

  1. Set non-required flag for texture filename attrib 

    Fix problems with loading URDF with empty texture tag from baxter's
dynamically loaded URDF on the param server.
    @rethink-rlinsalata
    rethink-rlinsalata committed Jan 30, 2015
    Copy the full SHA
    8299286 View commit details

Commits on Jun 29, 2015

  1. Add joint values for FK and Jacobian

    @buschbapti
    buschbapti committed Jun 29, 2015
    Copy the full SHA
    c071b2a View commit details

Commits on Jun 30, 2015

  1. Proper cast of the result of inverse_kinematics() + removed inconveni… 

    …ent print
    Yoan Mollard committed Jun 30, 2015
    Copy the full SHA
    3f9c0a4 View commit details

Commits on Jul 28, 2015

  1. Avoid code duplication, thanks @rethink-imcmahon

    Yoan Mollard committed Jul 28, 2015
    Copy the full SHA
    4235b03 View commit details

  2. Merge pull request #1 from baxter-flowers/master 

    Allow providing a seed for FK and Jacobian + cast IK result into a list of joint values
    Ian McMahon committed Jul 28, 2015
    Copy the full SHA
    8b95af3 View commit details
Showing with 41 additions and 29 deletions.
  1. +1 −1 package.xml
  2. +27 −24 src/baxter_pykdl/baxter_pykdl.py
  3. +13 −4 src/urdf_parser_py/urdf.py
2 changes: 1 addition & 1 deletion package.xml
View file
Original file line number Diff line number Diff line change
@@ -26,7 +26,7 @@
<build_depend>baxter_interface</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>urdfdom</build_depend>
<build_depend>liburdfdom-dev</build_depend>
<build_depend>python_orocos_kdl</build_depend>
<build_depend>tf</build_depend>

51 changes: 27 additions & 24 deletions src/baxter_pykdl/baxter_pykdl.py
View file
Original file line number Diff line number Diff line change
@@ -81,15 +81,19 @@ def print_kdl_chain(self):
for idx in xrange(self._arm_chain.getNrOfSegments()):
print '* ' + self._arm_chain.getSegment(idx).getName()

def joints_to_kdl(self, type):
def joints_to_kdl(self, type, values=None):
kdl_array = PyKDL.JntArray(self._num_jnts)

if type == 'positions':
cur_type_values = self._limb_interface.joint_angles()
elif type == 'velocities':
cur_type_values = self._limb_interface.joint_velocities()
elif type == 'torques':
cur_type_values = self._limb_interface.joint_efforts()
if values is None:
if type == 'positions':
cur_type_values = self._limb_interface.joint_angles()
elif type == 'velocities':
cur_type_values = self._limb_interface.joint_velocities()
elif type == 'torques':
cur_type_values = self._limb_interface.joint_efforts()
else:
cur_type_values = values

for idx, name in enumerate(self._joint_names):
kdl_array[idx] = cur_type_values[name]
if type == 'velocities':
@@ -103,19 +107,19 @@ def kdl_to_mat(self, data):
mat[i,j] = data[i,j]
return mat

def forward_position_kinematics(self):
def forward_position_kinematics(self,joint_values=None):
end_frame = PyKDL.Frame()
self._fk_p_kdl.JntToCart(self.joints_to_kdl('positions'),
self._fk_p_kdl.JntToCart(self.joints_to_kdl('positions',joint_values),
end_frame)
pos = end_frame.p
rot = PyKDL.Rotation(end_frame.M)
rot = rot.GetQuaternion()
return np.array([pos[0], pos[1], pos[2],
rot[0], rot[1], rot[2], rot[3]])

def forward_velocity_kinematics(self):
def forward_velocity_kinematics(self,joint_velocities=None):
end_frame = PyKDL.FrameVel()
self._fk_v_kdl.JntToCart(self.joints_to_kdl('velocities'),
self._fk_v_kdl.JntToCart(self.joints_to_kdl('velocities',joint_velocities),
end_frame)
return end_frame.GetTwist()

@@ -143,30 +147,29 @@ def inverse_kinematics(self, position, orientation=None, seed=None):
result_angles = PyKDL.JntArray(self._num_jnts)

if self._ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0:
result = np.array(result_angles)
result = np.array(list(result_angles))
return result
else:
print 'No IK Solution Found'
return None

def jacobian(self):
def jacobian(self,joint_values=None):
jacobian = PyKDL.Jacobian(self._num_jnts)
self._jac_kdl.JntToJac(self.joints_to_kdl('positions'), jacobian)
self._jac_kdl.JntToJac(self.joints_to_kdl('positions',joint_values), jacobian)
return self.kdl_to_mat(jacobian)

def jacobian_transpose(self):
return self.jacobian().T
def jacobian_transpose(self,joint_values=None):
return self.jacobian(joint_values).T

def jacobian_pseudo_inverse(self):
return np.linalg.pinv(self.jacobian())
def jacobian_pseudo_inverse(self,joint_values=None):
return np.linalg.pinv(self.jacobian(joint_values))


def inertia(self):
def inertia(self,joint_values=None):
inertia = PyKDL.JntSpaceInertiaMatrix(self._num_jnts)
self._dyn_kdl.JntToMass(self.joints_to_kdl('positions'), inertia)
self._dyn_kdl.JntToMass(self.joints_to_kdl('positions',joint_values), inertia)
return self.kdl_to_mat(inertia)

def cart_inertia(self):
js_inertia = self.inertia()
jacobian = self.jacobian()
def cart_inertia(self,joint_values=None):
js_inertia = self.inertia(joint_values)
jacobian = self.jacobian(joint_values)
return np.linalg.inv(jacobian * np.linalg.inv(js_inertia) * jacobian.T)
17 changes: 13 additions & 4 deletions src/urdf_parser_py/urdf.py
View file
Original file line number Diff line number Diff line change
@@ -149,7 +149,7 @@ def __init__(self, filename = None):
self.filename = filename

xmlr.reflect(Texture, params = [
xmlr.Attribute('filename', str)
xmlr.Attribute('filename', str, required=False)
])


@@ -203,6 +203,13 @@ def to_matrix(self):
xmlr.reflect(Inertia, params = [xmlr.Attribute(key, float) for key in Inertia.KEYS])


class Gravity(xmlr.Object):
def __init__(self):
pass

xmlr.reflect(Gravity, params = [
])

class Inertial(xmlr.Object):
def __init__(self, mass = 0.0, inertia = None, origin=None):
self.mass = mass
@@ -313,19 +320,21 @@ def joint_type(self, value): self.type = value


class Link(xmlr.Object):
def __init__(self, name=None, visual=None, inertial=None, collision=None, origin = None):
def __init__(self, name=None, visual=None, inertial=None, collision=None, origin = None, gravity = None):
self.name = name
self.visual = visual
self.inertial = inertial
self.collision = collision
self.origin = origin
self.gravity = gravity

xmlr.reflect(Link, params = [
name_attribute,
origin_element,
xmlr.Element('inertial', Inertial, False),
xmlr.Element('visual', Visual, False),
xmlr.Element('collision', Collision, False)
xmlr.Element('collision', Collision, False),
xmlr.Element('gravity', Gravity, False)
])


@@ -462,4 +471,4 @@ def from_parameter_server(cls, key = 'robot_description'):
# Make an alias
URDF = Robot

xmlr.end_namespace()
xmlr.end_namespace()