{"action":"created","issue":{"url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1138","repository_url":"https://api.github.com/repos/bulletphysics/bullet3","labels_url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1138/labels{/name}","comments_url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1138/comments","events_url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1138/events","html_url":"https://github.com/bulletphysics/bullet3/issues/1138","id":230238742,"number":1138,"title":"geometry, linear algebra, realtime fwd+inv kinematics & dynamics","user":{"login":"ahundt","id":55744,"avatar_url":"https://avatars2.githubusercontent.com/u/55744?v=3","gravatar_id":"","url":"https://api.github.com/users/ahundt","html_url":"https://github.com/ahundt","followers_url":"https://api.github.com/users/ahundt/followers","following_url":"https://api.github.com/users/ahundt/following{/other_user}","gists_url":"https://api.github.com/users/ahundt/gists{/gist_id}","starred_url":"https://api.github.com/users/ahundt/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ahundt/subscriptions","organizations_url":"https://api.github.com/users/ahundt/orgs","repos_url":"https://api.github.com/users/ahundt/repos","events_url":"https://api.github.com/users/ahundt/events{/privacy}","received_events_url":"https://api.github.com/users/ahundt/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"assignees":[],"milestone":null,"comments":3,"created_at":"2017-05-21T18:30:10Z","updated_at":"2017-05-23T01:51:37Z","closed_at":null,"body":"\r\n**tl;dr** These are high level python/robot control API suggestions. Feel free to close if not applicable.\r\n\r\nA [comment in issue 1136](https://github.com/bulletphysics/bullet3/issues/1136#issuecomment-302944843) mentioned plans for generic linear algebra tools for pybullet. I know bullet makes use of Featherstone, so the Tasks libraries may be good for a numpy style python API or perhaps just design ideas:\r\n\r\n```python\r\nimport numpy as np\r\nimport eigen3 as e\r\nimport spacevecalg as sva\r\nimport rbdyn as rbd\r\n\r\n# Featherstone Plucker transform, much like translation + quaternion pair\r\n\r\nX_O_foot = sva.PTransformd(sva.RotZ(np.pi)*sva.RotX(-np.pi/2.), e.Vector3d(0.5, -0.3, 0.))\r\nX_O_CoM = sva.PTransformd(e.Vector3d(-0.2, 0.8, 0.))\r\n\r\n# Spherical joint & Quaternion\r\n\r\njSph = rbd.Joint(rbd.Joint.Spherical, True, 0, 'jSph')\r\njointResume(jSph)\r\nq = e.Quaterniond(np.pi/4., e.Vector3d(1., 0., 0.5).normalized())\r\nqParam = [q.w(), q.x(), q.y(), q.z()]\r\nprint 'rotation:'\r\nprint jSph.pose(qParam).rotation()\r\nprint 'angular motion:', jSph.motion([1., 0.2, 0.5]).angular().transpose()\r\n\r\n# Creating Rigid Bodies\r\n\r\nb0m = 2. # 2kg\r\nb0c = e.Vector3d(0.5, 0.5, 0.) # center of mass at [0.5, 0.5, 0.] from the body origin\r\nb0rI = e.Matrix3d.Identity() # rotational inertia at body origin\r\n# here the second argument is the body first moment of mass (h = m c)\r\nb0I = sva.RBInertia(b0m, b0m*b0c, b0rI)\r\n# first body constructor that take sva.RBInertia as firt param\r\nb0 = rbd.Body(b0I, 0, 'b0')\r\n\r\nb1m = 4. # 4kg\r\nb1c = e.Vector3d(0., 0.5, 0.) # center of mass at [0., 0.5, 0.] from the body origin\r\nb1rI = e.Matrix3d.Identity() # rotational inertia at body origin\r\n# here the second argument is the body center of mass (not the first moment of mass)\r\nb1 = rbd.Body(b1m, b1c, b1rI, 1, 'b1')\r\n\r\n# note that in the latest api only string ids like 'b2'\r\n# are used, none of the manual numerical ids seen here\r\nb2 = rbd.Body(1., e.Vector3d.Zero(), e.Matrix3d.Identity(), 2, 'b2')\r\nb3 = rbd.Body(1., e.Vector3d.Zero(), e.Matrix3d.Identity(), 3, 'b3')\r\nb4 = rbd.Body(1., e.Vector3d.Zero(), e.Matrix3d.Identity(), 4, 'b4')\r\nb5 = rbd.Body(1., e.Vector3d.Zero(), e.Matrix3d.Identity(), 5, 'b5')\r\n\r\n# Inverse Dynamics with Gravity Comp\r\n# assume some multibody system is defined with mb, a list of joints, etc.\r\nmbcID = rbd.MultiBodyConfig(mb)\r\nmbcID.zero(mb)\r\nmbcID.q = mbc.q\r\n\r\nrbd.forwardKinematics(mb, mbcID)\r\nrbd.forwardVelocity(mb, mbcID)\r\nID = rbd.InverseDynamics(mb)\r\nID.inverseDynamics(mb, mbcID)\r\n\r\njointTorque = map(list, mbcID.jointTorque)\r\nprint 'jointTorque:', jointTorque\r\n\r\n# jointTorque: [[], [-24.525], [4.905000000000001], [8.047710038720992e-17], [8.495709211125343], [0.0, 0.0, 0.0]]\r\n\r\n```\r\n\r\n **Clean tools to define and solve for goals & constraints for any part of a robot**, such as end effectors, which are then solved for via Quadratic Programming based approaches:\r\n * Articular position, speed and torque limits, Collision avoidance\r\n * Multi-robot contact (can solve problems involving multiple robots in contact)\r\n * Task Target Goals (Inverse kin/dyn):\r\n    * Link Position/Orientation, Velocity, Center of Mass (CoM) target, Momentum, Contact force target\r\n\r\nUnfortunately I don't have a python example on hand so here is a\r\n[C++ example of Tasks using position and orientation goals](https://github.com/ahundt/grl/blob/master/include/grl/vrep/InverseKinematicsVrepPlugin.hpp#L616)\r\n```C++\r\n        tasks::qp::QPSolver solver;\r\n        \r\n        ////////////////////////////////////////////////////\r\n        // Set position goal of the arm\r\n        sva::PTransformd targetWorldTransform;\r\n        // ...snip...\r\n\r\n        // mbs defines the robot structure\r\n        // mbcs defines where the robot is now\r\n        tasks::qp::PositionTask posTask(rbd_mbs_, simulatedRobotIndex, ikGroupTipName_,targetWorldTransform.translation());\r\n        tasks::qp::SetPointTask posTaskSp(rbd_mbs_, simulatedRobotIndex, &posTask, 50., 10.);\r\n        tasks::qp::OrientationTask oriTask(rbd_mbs_,simulatedRobotIndex, ikGroupTipName_,targetWorldTransform.rotation());\r\n        tasks::qp::SetPointTask oriTaskSp(rbd_mbs_, simulatedRobotIndex, &oriTask, 50., 1.);\r\n        tasks::qp::PostureTask postureTask(rbd_mbs_,simulatedRobotIndex,rbd_preferred_mbcs_[simulatedRobotIndex].q,1,0.01);\r\n\r\n        ////////////////////////////////////\r\n        // Set joint limits\r\n\r\n        // ...snip...\r\n\r\n        tasks::qp::DamperJointLimitsConstr dampJointConstr(rbd_mbs_, simulatedRobotIndex, {lBound, uBound},{lVelBound, uVelBound}, 0.125, 0.025, 1., simulationTimeStep);\r\n\r\n        // Test add*Constraint\r\n        dampJointConstr.addToSolver(solver);\r\n        BOOST_VERIFY(solver.nrBoundConstraints() == 1);\r\n        BOOST_VERIFY(solver.nrConstraints() == 1);\r\n\r\n        solver.nrVars(rbd_mbs_, {}, {});\r\n        solver.updateConstrSize();\r\n\r\n        solver.addTask(&posTaskSp);\r\n        solver.addTask(&oriTaskSp);\r\n        solver.addTask(&postureTask);\r\n\r\n        // ...snip...\r\n        solver.solve(rbd_mbs_, rbd_mbcs_);\r\n        // ...snip...\r\n```\r\n[Video of Robot Following Simulated Real Time Constrained Optimization Motion Task in V-REP](https://youtu.be/nxYoOChgMKg)\r\n\r\n- Tasks Library Stack from high level to low level APIs\r\n    - [github.com/jrl-umi3218/sva_rbdyn_tutorials](https://github.com/jrl-umi3218/sva_rbdyn_tutorials) Python tutorials\r\n    - [Tasks](https://github.com/jrl-umi3218/Tasks) Quadratic Programming based realtime arm control with position, velocity, contact forces, etc.\r\n    - [RBDyn](https://github.com/jrl-umi3218/RBDyn) Featherstone Rigid Body Dynamics\r\n    - [SpaceVecAlg](https://github.com/jrl-umi3218/SpaceVecAlg) Featherstone Spatial Vector Algebra\r\n    - [shell script to build/install the whole stack](https://github.com/ahundt/robotics_setup/blob/master/robotics_tasks.sh)\r\n\r\n\r\n"},"comment":{"url":"https://api.github.com/repos/bulletphysics/bullet3/issues/comments/303267544","html_url":"https://github.com/bulletphysics/bullet3/issues/1138#issuecomment-303267544","issue_url":"https://api.github.com/repos/bulletphysics/bullet3/issues/1138","id":303267544,"user":{"login":"ahundt","id":55744,"avatar_url":"https://avatars2.githubusercontent.com/u/55744?v=3","gravatar_id":"","url":"https://api.github.com/users/ahundt","html_url":"https://github.com/ahundt","followers_url":"https://api.github.com/users/ahundt/followers","following_url":"https://api.github.com/users/ahundt/following{/other_user}","gists_url":"https://api.github.com/users/ahundt/gists{/gist_id}","starred_url":"https://api.github.com/users/ahundt/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/ahundt/subscriptions","organizations_url":"https://api.github.com/users/ahundt/orgs","repos_url":"https://api.github.com/users/ahundt/repos","events_url":"https://api.github.com/users/ahundt/events{/privacy}","received_events_url":"https://api.github.com/users/ahundt/received_events","type":"User","site_admin":false},"created_at":"2017-05-23T01:51:37Z","updated_at":"2017-05-23T01:51:37Z","body":"looks cool, glad I have it running but I meant something a bit different. which could potentially make pybullet scene calculations and image generation 10x faster than if they were implemented the way numpy is. Tensorflow happens to be used for machine learning, but really it is just a math library that creates a graph of mathematical operations saved in a protobuf format that can be run much faster than the python interpreter. \r\n\r\nThis alternative design is one where pybullet could supply tensorflow ops to retrieve and set transforms, load segmentation images, rendered images, do quaternion math, etc. In other words the images might never need to be copied from c to python.\r\n\r\nEssentially, use the ability to [implement new ops](https://www.tensorflow.org/extend/adding_an_op) and add [custom data readers](https://www.tensorflow.org/extend/new_data_formats) to define functions under the namespace `pybullet.tf`.\r\n\r\nexample:\r\n```python\r\nimport tensorflow as tf\r\n# new pybullet tensorflow functionality\r\nimport pybullet.tf as pbtf \r\n\r\n# op that when run returns true if the simulation is running,\r\n# false otherwise.\r\nsimulation_running_op = pbtf.SimulationIsRunning()\r\n\r\n# this returns a tensorflow op, not a numpy array\r\ncamera_op = pbtf.getCameraImage(width,height)\r\n\r\n# can have some configuration params for how it works...\r\nrandom_camera_op = pbtf.randomizeCameraPose(camera_op)\r\n\r\n# this is some user defined function \r\n# that loads some regular tf model\r\n# and weights from disk, let's say it\r\n# is predicting segmentation\r\nmodel_tf_op = my_load_model(random_camera_op)\r\n\r\nsegmentation_op = pbtf.getCameraSegmentation(random_camera_op)\r\n\r\n# some train function\r\ntraining_op = my_train_fn(model_tf_op, segmentation_op)\r\n\r\nwith tf.Session() as sess:\r\n  train_with_pybullet = tf.while_loop(training_op, simulation_running_op)\r\n  # probably needs some time limit parameter or something\r\n  sess.run(train_with_pybullet)  \r\n\r\n```\r\n\r\nAdditional ops can exist for getting transforms between objects, creating and configuring qp solvers, basically any functionality the user wants, but they get a huge performance boost from it being tensorflow ops instead of directly running python code.\r\n\r\nHopefully my explanation made sense?"}}
{"actor":{"display_login":"ahundt"}}